From 74d8663c106f345913e5d220e1ab8de29800d858 Mon Sep 17 00:00:00 2001 From: mik-p Date: Tue, 23 Sep 2025 15:13:53 +1000 Subject: [PATCH 01/37] update changelog --- TODO.md | 18 +++++++++++++++++- capabilities2/package.xml | 12 ++++++------ changelog.md | 11 +++++++++++ 3 files changed, 34 insertions(+), 7 deletions(-) diff --git a/TODO.md b/TODO.md index 0808968..219be4d 100644 --- a/TODO.md +++ b/TODO.md @@ -8,7 +8,12 @@ - [x] BUG: escape db function variables - [ ] close or change communication to launch proxy so that it can't be accessed from ros network - [ ] BUG: fix issue with connecting to services and actions started using launch proxy - +- [ ] Launch proxy does not work, maybe remove and note as future work, or suggest alternatives +- [x] add descriptions to packages with TODO in package.xml +- [ ] standardise trigger prototype +- [x] bump cmake min version to 3.16 +- [ ] check thread safety for runner execution threads +- [ ] add note on threaded execution in base trigger function ## Features @@ -17,3 +22,14 @@ - [ ] implement provider definition handling in runner - [ ] move to established db handler lib - [ ] better bt runner impl +- [ ] db traits: identifiable, modifiable, soft_deleteable, header, remappable, predicateable + +## Refactoring + +- [x] remove fan out project work +- [x] merge various system runners into base runner package +- [ ] custom logger needs to be removed +- [ ] events should be incorporated as core function +- [ ] server, runner base, api have event +- [ ] increment package versions +- [ ] interfaces and providers for cap caps diff --git a/capabilities2/package.xml b/capabilities2/package.xml index a012c4d..79d0029 100644 --- a/capabilities2/package.xml +++ b/capabilities2/package.xml @@ -1,25 +1,25 @@ capabilities2 - 0.0.1 + 0.2.0 - meta-package for the capabilities2 interface + meta-package for the capabilities2 framework Michael Pritchard - mik-p Kalana Ratnayake Michael Pritchard + Kalana Ratnayake MIT - https://github.com/AIResearchLab/capabilities2 + https://github.com/CollaborativeRoboticsLab/capabilities2 capabilities2_msgs - capabilities2_server + capabilities2_events capabilities2_runner - capabilities2_launch_proxy + capabilities2_server ament_cmake diff --git a/changelog.md b/changelog.md index 06d8dbb..fc57043 100644 --- a/changelog.md +++ b/changelog.md @@ -1,5 +1,16 @@ # Changelog +## 0.2.0 (2025-09-16) + +- add new service types +- upgrade event subsystem +- implement basic db traits +- add system and capability runner plugins +- threaded runner execution +- runners can be chained through trigger +- triggering between runners handled by dynamic link list style structure +- event system upgraded and refactored to be more generic and easier to use + ## 0.1.2 (2024-09-20) - working on bt runner plugins From 7cf923e51745bd7d85f270d48ad66f29207267eb Mon Sep 17 00:00:00 2001 From: mik-p Date: Tue, 23 Sep 2025 15:39:41 +1000 Subject: [PATCH 02/37] merge system and cap level runner plugins into base package --- .../capabilities2_runner}/get_runner.hpp | 4 +- .../input_multiplex_all_runner.hpp | 0 .../input_multiplex_any_runner.hpp | 0 .../capabilities2_runner/launch_runner.hpp | 2 +- .../multiplex_base_runner.hpp | 10 ++- .../interfaces/CapabilityGetRunner.yaml | 0 .../interfaces/InputMultiplexAllRunner.yaml | 0 .../interfaces/InputMultiplexAnyRunner.yaml | 0 capabilities2_runner/plugins.xml | 17 +++++ .../providers/CapabilityGetRunner.yaml | 0 .../providers/InputMultiplexAllRunner.yaml | 0 .../providers/InputMultiplexAnyRunner.yaml | 0 .../src/capability_get_runner.cpp | 2 +- ...{standard_runners.cpp => dummy_runner.cpp} | 11 +-- capabilities2_runner/src/launch_runner.cpp | 6 ++ .../src/multiplex_all_runner.cpp | 2 - .../src/multiplex_any_runner.cpp | 6 ++ .../.clang-format | 64 ----------------- .../CMakeLists.txt | 69 ------------------ capabilities2_runner_capabilities/package.xml | 43 ----------- capabilities2_runner_capabilities/plugins.xml | 7 -- capabilities2_runner_system/.clang-format | 64 ----------------- capabilities2_runner_system/CMakeLists.txt | 71 ------------------- capabilities2_runner_system/package.xml | 55 -------------- capabilities2_runner_system/plugins.xml | 14 ---- 25 files changed, 49 insertions(+), 398 deletions(-) rename {capabilities2_runner_capabilities/include/capabilities2_runner_capabilities => capabilities2_runner/include/capabilities2_runner}/get_runner.hpp (97%) rename {capabilities2_runner_system/include/capabilities2_runner_system => capabilities2_runner/include/capabilities2_runner}/input_multiplex_all_runner.hpp (100%) rename {capabilities2_runner_system/include/capabilities2_runner_system => capabilities2_runner/include/capabilities2_runner}/input_multiplex_any_runner.hpp (100%) rename {capabilities2_runner_system/include/capabilities2_runner_system => capabilities2_runner/include/capabilities2_runner}/multiplex_base_runner.hpp (94%) rename {capabilities2_runner_capabilities => capabilities2_runner}/interfaces/CapabilityGetRunner.yaml (100%) rename {capabilities2_runner_system => capabilities2_runner}/interfaces/InputMultiplexAllRunner.yaml (100%) rename {capabilities2_runner_system => capabilities2_runner}/interfaces/InputMultiplexAnyRunner.yaml (100%) rename {capabilities2_runner_capabilities => capabilities2_runner}/providers/CapabilityGetRunner.yaml (100%) rename {capabilities2_runner_system => capabilities2_runner}/providers/InputMultiplexAllRunner.yaml (100%) rename {capabilities2_runner_system => capabilities2_runner}/providers/InputMultiplexAnyRunner.yaml (100%) rename capabilities2_runner_capabilities/src/capabilities2_runner.cpp => capabilities2_runner/src/capability_get_runner.cpp (78%) rename capabilities2_runner/src/{standard_runners.cpp => dummy_runner.cpp} (83%) create mode 100644 capabilities2_runner/src/launch_runner.cpp rename capabilities2_runner_system/src/capabilities2_runner.cpp => capabilities2_runner/src/multiplex_all_runner.cpp (62%) create mode 100644 capabilities2_runner/src/multiplex_any_runner.cpp delete mode 100644 capabilities2_runner_capabilities/.clang-format delete mode 100644 capabilities2_runner_capabilities/CMakeLists.txt delete mode 100644 capabilities2_runner_capabilities/package.xml delete mode 100644 capabilities2_runner_capabilities/plugins.xml delete mode 100644 capabilities2_runner_system/.clang-format delete mode 100644 capabilities2_runner_system/CMakeLists.txt delete mode 100644 capabilities2_runner_system/package.xml delete mode 100644 capabilities2_runner_system/plugins.xml diff --git a/capabilities2_runner_capabilities/include/capabilities2_runner_capabilities/get_runner.hpp b/capabilities2_runner/include/capabilities2_runner/get_runner.hpp similarity index 97% rename from capabilities2_runner_capabilities/include/capabilities2_runner_capabilities/get_runner.hpp rename to capabilities2_runner/include/capabilities2_runner/get_runner.hpp index e6d70ad..113c002 100644 --- a/capabilities2_runner_capabilities/include/capabilities2_runner_capabilities/get_runner.hpp +++ b/capabilities2_runner/include/capabilities2_runner/get_runner.hpp @@ -9,7 +9,7 @@ #include #include -namespace capabilities2_runner +namespace capabilities2_runner_capabilities { /** @@ -114,4 +114,4 @@ class CapabilityGetRunner : public ServiceRunner::SharedPtr stop_service_client_; }; -} // namespace capabilities2_runner \ No newline at end of file +} // namespace capabilities2_runner diff --git a/capabilities2_runner_system/include/capabilities2_runner_system/multiplex_base_runner.hpp b/capabilities2_runner/include/capabilities2_runner/multiplex_base_runner.hpp similarity index 94% rename from capabilities2_runner_system/include/capabilities2_runner_system/multiplex_base_runner.hpp rename to capabilities2_runner/include/capabilities2_runner/multiplex_base_runner.hpp index 10d2c61..7eb0d34 100644 --- a/capabilities2_runner_system/include/capabilities2_runner_system/multiplex_base_runner.hpp +++ b/capabilities2_runner/include/capabilities2_runner/multiplex_base_runner.hpp @@ -7,6 +7,13 @@ namespace capabilities2_runner { + +/** + * @brief Multiplex Base Runner + * + * Base class for inter-runner connections that require multiplexing of inputs + * + */ class MultiplexBaseRunner : public RunnerBase { public: @@ -90,4 +97,5 @@ class MultiplexBaseRunner : public RunnerBase // completed executions std::map completed_executions; }; -} // namespace capabilities2_runner \ No newline at end of file + +} // namespace capabilities2_runner diff --git a/capabilities2_runner_capabilities/interfaces/CapabilityGetRunner.yaml b/capabilities2_runner/interfaces/CapabilityGetRunner.yaml similarity index 100% rename from capabilities2_runner_capabilities/interfaces/CapabilityGetRunner.yaml rename to capabilities2_runner/interfaces/CapabilityGetRunner.yaml diff --git a/capabilities2_runner_system/interfaces/InputMultiplexAllRunner.yaml b/capabilities2_runner/interfaces/InputMultiplexAllRunner.yaml similarity index 100% rename from capabilities2_runner_system/interfaces/InputMultiplexAllRunner.yaml rename to capabilities2_runner/interfaces/InputMultiplexAllRunner.yaml diff --git a/capabilities2_runner_system/interfaces/InputMultiplexAnyRunner.yaml b/capabilities2_runner/interfaces/InputMultiplexAnyRunner.yaml similarity index 100% rename from capabilities2_runner_system/interfaces/InputMultiplexAnyRunner.yaml rename to capabilities2_runner/interfaces/InputMultiplexAnyRunner.yaml diff --git a/capabilities2_runner/plugins.xml b/capabilities2_runner/plugins.xml index 756596f..b2e2d7a 100644 --- a/capabilities2_runner/plugins.xml +++ b/capabilities2_runner/plugins.xml @@ -1,4 +1,16 @@ + + + A plugin that Acts as a multiplexer for multiple input streams, allowing them to be processed in a single runner. + It can handle multiple input streams and route them to the appropriate output. + + + + + A plugin that Acts as a multiplexer for multiple input streams, allowing them to be processed in a single runner. + It can handle multiple input streams and route them to the appropriate output. + + A plugin that provides capabilities2 launch based runner @@ -9,4 +21,9 @@ A plugin that provides a dummy-runner to test capabilities2 + + + A plugin that request capabilities from the capabilities server and transfers to an LLM + + diff --git a/capabilities2_runner_capabilities/providers/CapabilityGetRunner.yaml b/capabilities2_runner/providers/CapabilityGetRunner.yaml similarity index 100% rename from capabilities2_runner_capabilities/providers/CapabilityGetRunner.yaml rename to capabilities2_runner/providers/CapabilityGetRunner.yaml diff --git a/capabilities2_runner_system/providers/InputMultiplexAllRunner.yaml b/capabilities2_runner/providers/InputMultiplexAllRunner.yaml similarity index 100% rename from capabilities2_runner_system/providers/InputMultiplexAllRunner.yaml rename to capabilities2_runner/providers/InputMultiplexAllRunner.yaml diff --git a/capabilities2_runner_system/providers/InputMultiplexAnyRunner.yaml b/capabilities2_runner/providers/InputMultiplexAnyRunner.yaml similarity index 100% rename from capabilities2_runner_system/providers/InputMultiplexAnyRunner.yaml rename to capabilities2_runner/providers/InputMultiplexAnyRunner.yaml diff --git a/capabilities2_runner_capabilities/src/capabilities2_runner.cpp b/capabilities2_runner/src/capability_get_runner.cpp similarity index 78% rename from capabilities2_runner_capabilities/src/capabilities2_runner.cpp rename to capabilities2_runner/src/capability_get_runner.cpp index bcdf964..55a61a7 100644 --- a/capabilities2_runner_capabilities/src/capabilities2_runner.cpp +++ b/capabilities2_runner/src/capability_get_runner.cpp @@ -1,6 +1,6 @@ #include #include -#include +#include // register runner plugins PLUGINLIB_EXPORT_CLASS(capabilities2_runner::CapabilityGetRunner, capabilities2_runner::RunnerBase); diff --git a/capabilities2_runner/src/standard_runners.cpp b/capabilities2_runner/src/dummy_runner.cpp similarity index 83% rename from capabilities2_runner/src/standard_runners.cpp rename to capabilities2_runner/src/dummy_runner.cpp index 58ca354..2574e51 100644 --- a/capabilities2_runner/src/standard_runners.cpp +++ b/capabilities2_runner/src/dummy_runner.cpp @@ -1,9 +1,15 @@ #include #include -#include namespace capabilities2_runner { +/** + * @brief Dummy runner + * + * A sample runner that can be used to test the functionality of capabilities server. + * It does not perform any real action but logs messages when started and stopped. + * + */ class DummyRunner : public RunnerBase { public: @@ -41,8 +47,5 @@ class DummyRunner : public RunnerBase } // namespace capabilities2_runner -// register runner plugins -PLUGINLIB_EXPORT_CLASS(capabilities2_runner::LaunchRunner, capabilities2_runner::RunnerBase) - // dummy runner PLUGINLIB_EXPORT_CLASS(capabilities2_runner::DummyRunner, capabilities2_runner::RunnerBase) diff --git a/capabilities2_runner/src/launch_runner.cpp b/capabilities2_runner/src/launch_runner.cpp new file mode 100644 index 0000000..d001801 --- /dev/null +++ b/capabilities2_runner/src/launch_runner.cpp @@ -0,0 +1,6 @@ +#include +#include +#include + +// register runner plugins +PLUGINLIB_EXPORT_CLASS(capabilities2_runner_capabilities::LaunchRunner, capabilities2_runner::RunnerBase) diff --git a/capabilities2_runner_system/src/capabilities2_runner.cpp b/capabilities2_runner/src/multiplex_all_runner.cpp similarity index 62% rename from capabilities2_runner_system/src/capabilities2_runner.cpp rename to capabilities2_runner/src/multiplex_all_runner.cpp index 114133e..9896fc0 100644 --- a/capabilities2_runner_system/src/capabilities2_runner.cpp +++ b/capabilities2_runner/src/multiplex_all_runner.cpp @@ -1,8 +1,6 @@ #include #include #include -#include // register runner plugins PLUGINLIB_EXPORT_CLASS(capabilities2_runner::InputMultiplexAllRunner, capabilities2_runner::RunnerBase); -PLUGINLIB_EXPORT_CLASS(capabilities2_runner::InputMultiplexAnyRunner, capabilities2_runner::RunnerBase); \ No newline at end of file diff --git a/capabilities2_runner/src/multiplex_any_runner.cpp b/capabilities2_runner/src/multiplex_any_runner.cpp new file mode 100644 index 0000000..7b86da2 --- /dev/null +++ b/capabilities2_runner/src/multiplex_any_runner.cpp @@ -0,0 +1,6 @@ +#include +#include +#include + +// register runner plugins +PLUGINLIB_EXPORT_CLASS(capabilities2_runner::InputMultiplexAllRunner, capabilities2_runner::RunnerBase); diff --git a/capabilities2_runner_capabilities/.clang-format b/capabilities2_runner_capabilities/.clang-format deleted file mode 100644 index d36804f..0000000 --- a/capabilities2_runner_capabilities/.clang-format +++ /dev/null @@ -1,64 +0,0 @@ - -BasedOnStyle: Google -AccessModifierOffset: -2 -ConstructorInitializerIndentWidth: 2 -AlignEscapedNewlinesLeft: false -AlignTrailingComments: true -AllowAllParametersOfDeclarationOnNextLine: false -AllowShortIfStatementsOnASingleLine: false -AllowShortLoopsOnASingleLine: false -AllowShortFunctionsOnASingleLine: None -AlwaysBreakTemplateDeclarations: true -AlwaysBreakBeforeMultilineStrings: false -BreakBeforeBinaryOperators: false -BreakBeforeTernaryOperators: false -BreakConstructorInitializersBeforeComma: true -BinPackParameters: true -ColumnLimit: 120 -ConstructorInitializerAllOnOneLineOrOnePerLine: true -DerivePointerBinding: false -PointerBindsToType: true -ExperimentalAutoDetectBinPacking: false -IndentCaseLabels: true -MaxEmptyLinesToKeep: 1 -NamespaceIndentation: None -ObjCSpaceBeforeProtocolList: true -PenaltyBreakBeforeFirstCallParameter: 19 -PenaltyBreakComment: 60 -PenaltyBreakString: 1 -PenaltyBreakFirstLessLess: 1000 -PenaltyExcessCharacter: 1000 -PenaltyReturnTypeOnItsOwnLine: 90 -SpacesBeforeTrailingComments: 2 -Cpp11BracedListStyle: false -Standard: Auto -IndentWidth: 2 -TabWidth: 2 -UseTab: Never -IndentFunctionDeclarationAfterType: false -SpacesInParentheses: false -SpacesInAngles: false -SpaceInEmptyParentheses: false -SpacesInCStyleCastParentheses: false -SpaceAfterControlStatementKeyword: true -SpaceBeforeAssignmentOperators: true -ContinuationIndentWidth: 4 -SortIncludes: false -SpaceAfterCStyleCast: false - -# Configure each individual brace in BraceWrapping -BreakBeforeBraces: Custom - -# Control of individual brace wrapping cases -BraceWrapping: { - AfterClass: 'true' - AfterControlStatement: 'true' - AfterEnum : 'true' - AfterFunction : 'true' - AfterNamespace : 'true' - AfterStruct : 'true' - AfterUnion : 'true' - BeforeCatch : 'true' - BeforeElse : 'true' - IndentBraces : 'false' -} diff --git a/capabilities2_runner_capabilities/CMakeLists.txt b/capabilities2_runner_capabilities/CMakeLists.txt deleted file mode 100644 index 6aa29c9..0000000 --- a/capabilities2_runner_capabilities/CMakeLists.txt +++ /dev/null @@ -1,69 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(capabilities2_runner_capabilities) - -# Default to C++17 -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) -endif() - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -find_package(ament_cmake REQUIRED) -find_package(rclcpp REQUIRED) -find_package(rclcpp_action REQUIRED) -find_package(pluginlib REQUIRED) -find_package(capabilities2_msgs REQUIRED) -find_package(capabilities2_runner REQUIRED) -find_package(capabilities2_utils REQUIRED) -find_package(event_logger REQUIRED) -find_package(event_logger_msgs REQUIRED) -find_package(tinyxml2_vendor REQUIRED) -find_package(TinyXML2 REQUIRED) # provided by tinyxml2 upstream, or tinyxml2_vendor - -include_directories( - include -) - -add_library(${PROJECT_NAME} SHARED - src/capabilities2_runner.cpp -) - -ament_target_dependencies(${PROJECT_NAME} - rclcpp - rclcpp_action - pluginlib - capabilities2_msgs - capabilities2_runner - capabilities2_utils - event_logger - event_logger_msgs - TinyXML2 -) - -pluginlib_export_plugin_description_file(capabilities2_runner plugins.xml) - -install(TARGETS ${PROJECT_NAME} - EXPORT ${PROJECT_NAME} - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION bin -) - -install(DIRECTORY include/ - DESTINATION include -) - -install(DIRECTORY interfaces - DESTINATION share/${PROJECT_NAME} -) - -install(DIRECTORY providers - DESTINATION share/${PROJECT_NAME} -) - -ament_export_include_directories(include) -ament_export_libraries(${PROJECT_NAME}) - -ament_package() diff --git a/capabilities2_runner_capabilities/package.xml b/capabilities2_runner_capabilities/package.xml deleted file mode 100644 index 53bff7d..0000000 --- a/capabilities2_runner_capabilities/package.xml +++ /dev/null @@ -1,43 +0,0 @@ - - - - capabilities2_runner_capabilities - 0.0.0 - TODO: Package description - - - Kalana Ratnayake - Kalana Ratnayake - - Kalana Ratnayake - - TODO: License declaration - - ament_cmake - - ament_lint_auto - ament_lint_common - - rclcpp - rclcpp_action - pluginlib - std_msgs - capabilities2_msgs - capabilities2_runner - event_logger - event_logger_msgs - tinyxml2_vendor - - - ament_cmake - - - - interfaces/CapabilityGetRunner.yaml - - - - providers/CapabilityGetRunner.yaml - - - diff --git a/capabilities2_runner_capabilities/plugins.xml b/capabilities2_runner_capabilities/plugins.xml deleted file mode 100644 index f4ff0f8..0000000 --- a/capabilities2_runner_capabilities/plugins.xml +++ /dev/null @@ -1,7 +0,0 @@ - - - - A plugin that request capabilities from the capabilities server and transfers to an LLM - - - diff --git a/capabilities2_runner_system/.clang-format b/capabilities2_runner_system/.clang-format deleted file mode 100644 index d36804f..0000000 --- a/capabilities2_runner_system/.clang-format +++ /dev/null @@ -1,64 +0,0 @@ - -BasedOnStyle: Google -AccessModifierOffset: -2 -ConstructorInitializerIndentWidth: 2 -AlignEscapedNewlinesLeft: false -AlignTrailingComments: true -AllowAllParametersOfDeclarationOnNextLine: false -AllowShortIfStatementsOnASingleLine: false -AllowShortLoopsOnASingleLine: false -AllowShortFunctionsOnASingleLine: None -AlwaysBreakTemplateDeclarations: true -AlwaysBreakBeforeMultilineStrings: false -BreakBeforeBinaryOperators: false -BreakBeforeTernaryOperators: false -BreakConstructorInitializersBeforeComma: true -BinPackParameters: true -ColumnLimit: 120 -ConstructorInitializerAllOnOneLineOrOnePerLine: true -DerivePointerBinding: false -PointerBindsToType: true -ExperimentalAutoDetectBinPacking: false -IndentCaseLabels: true -MaxEmptyLinesToKeep: 1 -NamespaceIndentation: None -ObjCSpaceBeforeProtocolList: true -PenaltyBreakBeforeFirstCallParameter: 19 -PenaltyBreakComment: 60 -PenaltyBreakString: 1 -PenaltyBreakFirstLessLess: 1000 -PenaltyExcessCharacter: 1000 -PenaltyReturnTypeOnItsOwnLine: 90 -SpacesBeforeTrailingComments: 2 -Cpp11BracedListStyle: false -Standard: Auto -IndentWidth: 2 -TabWidth: 2 -UseTab: Never -IndentFunctionDeclarationAfterType: false -SpacesInParentheses: false -SpacesInAngles: false -SpaceInEmptyParentheses: false -SpacesInCStyleCastParentheses: false -SpaceAfterControlStatementKeyword: true -SpaceBeforeAssignmentOperators: true -ContinuationIndentWidth: 4 -SortIncludes: false -SpaceAfterCStyleCast: false - -# Configure each individual brace in BraceWrapping -BreakBeforeBraces: Custom - -# Control of individual brace wrapping cases -BraceWrapping: { - AfterClass: 'true' - AfterControlStatement: 'true' - AfterEnum : 'true' - AfterFunction : 'true' - AfterNamespace : 'true' - AfterStruct : 'true' - AfterUnion : 'true' - BeforeCatch : 'true' - BeforeElse : 'true' - IndentBraces : 'false' -} diff --git a/capabilities2_runner_system/CMakeLists.txt b/capabilities2_runner_system/CMakeLists.txt deleted file mode 100644 index aaec417..0000000 --- a/capabilities2_runner_system/CMakeLists.txt +++ /dev/null @@ -1,71 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(capabilities2_runner_system) - -# Default to C++17 -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) -endif() - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -find_package(ament_cmake REQUIRED) -find_package(rclcpp REQUIRED) -find_package(rclcpp_action REQUIRED) -find_package(pluginlib REQUIRED) -find_package(capabilities2_msgs REQUIRED) -find_package(capabilities2_runner REQUIRED) -find_package(capabilities2_utils REQUIRED) -find_package(fabric_msgs REQUIRED) -find_package(event_logger REQUIRED) -find_package(event_logger_msgs REQUIRED) -find_package(tinyxml2_vendor REQUIRED) -find_package(TinyXML2 REQUIRED) # provided by tinyxml2 upstream, or tinyxml2_vendor - -include_directories( - include -) - -add_library(${PROJECT_NAME} SHARED - src/capabilities2_runner.cpp -) - -ament_target_dependencies(${PROJECT_NAME} - rclcpp - rclcpp_action - pluginlib - capabilities2_msgs - fabric_msgs - capabilities2_runner - capabilities2_utils - event_logger - event_logger_msgs - TinyXML2 -) - -pluginlib_export_plugin_description_file(capabilities2_runner plugins.xml) - -install(TARGETS ${PROJECT_NAME} - EXPORT ${PROJECT_NAME} - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION bin -) - -install(DIRECTORY include/ - DESTINATION include -) - -install(DIRECTORY interfaces - DESTINATION share/${PROJECT_NAME} -) - -install(DIRECTORY providers - DESTINATION share/${PROJECT_NAME} -) - -ament_export_include_directories(include) -ament_export_libraries(${PROJECT_NAME}) - -ament_package() diff --git a/capabilities2_runner_system/package.xml b/capabilities2_runner_system/package.xml deleted file mode 100644 index 1af9a4e..0000000 --- a/capabilities2_runner_system/package.xml +++ /dev/null @@ -1,55 +0,0 @@ - - - - capabilities2_runner_system - 0.0.0 - TODO: Package description - - - Kalana Ratnayake - Kalana Ratnayake - - Kalana Ratnayake - - TODO: License declaration - - ament_cmake - - ament_lint_auto - ament_lint_common - - rclcpp - rclcpp_action - pluginlib - std_msgs - capabilities2_msgs - capabilities2_runner - capabilities2_utils - fabric_msgs - event_logger - event_logger_msgs - tinyxml2_vendor - - - ament_cmake - - - - interfaces/InputMultiplexAllRunner.yaml - - - - providers/InputMultiplexAllRunner.yaml - - - - - interfaces/InputMultiplexAnyRunner.yaml - - - - providers/InputMultiplexAnyRunner.yaml - - - - diff --git a/capabilities2_runner_system/plugins.xml b/capabilities2_runner_system/plugins.xml deleted file mode 100644 index bc05f3c..0000000 --- a/capabilities2_runner_system/plugins.xml +++ /dev/null @@ -1,14 +0,0 @@ - - - - A plugin that Acts as a multiplexer for multiple input streams, allowing them to be processed in a single runner. - It can handle multiple input streams and route them to the appropriate output. - - - - - A plugin that Acts as a multiplexer for multiple input streams, allowing them to be processed in a single runner. - It can handle multiple input streams and route them to the appropriate output. - - - From 69274e87227435ecf7169d09d2c644c8c3d3ae70 Mon Sep 17 00:00:00 2001 From: mik-p Date: Tue, 23 Sep 2025 15:41:05 +1000 Subject: [PATCH 03/37] merge util package function, remove util package --- capabilities2_utils/CMakeLists.txt | 15 --- .../capabilities2_utils/bond_client.hpp | 95 ------------------- .../capabilities2_utils/connection.hpp | 32 ------- .../capabilities2_utils/event_types.hpp | 47 --------- capabilities2_utils/package.xml | 23 ----- 5 files changed, 212 deletions(-) delete mode 100644 capabilities2_utils/CMakeLists.txt delete mode 100644 capabilities2_utils/include/capabilities2_utils/bond_client.hpp delete mode 100644 capabilities2_utils/include/capabilities2_utils/connection.hpp delete mode 100644 capabilities2_utils/include/capabilities2_utils/event_types.hpp delete mode 100644 capabilities2_utils/package.xml diff --git a/capabilities2_utils/CMakeLists.txt b/capabilities2_utils/CMakeLists.txt deleted file mode 100644 index db84b39..0000000 --- a/capabilities2_utils/CMakeLists.txt +++ /dev/null @@ -1,15 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(capabilities2_utils) - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -find_package(ament_cmake REQUIRED) - -install(DIRECTORY include/ - DESTINATION include -) - -ament_export_include_directories(include) -ament_package() diff --git a/capabilities2_utils/include/capabilities2_utils/bond_client.hpp b/capabilities2_utils/include/capabilities2_utils/bond_client.hpp deleted file mode 100644 index 3db48fd..0000000 --- a/capabilities2_utils/include/capabilities2_utils/bond_client.hpp +++ /dev/null @@ -1,95 +0,0 @@ -#pragma once -#include -#include -#include -#include - -class BondClient -{ -public: - /** - * @brief Construct a new Bond Client object - * - * @param node pointer to the node - * @param event_client pointer to the event client - * @param bond_id Bond id string - * @param bonds_topic Bond topic to be published - */ - BondClient(rclcpp::Node::SharedPtr node, std::shared_ptr event_client, const std::string &bond_id, const std::string &bonds_topic = "/capabilities/bond") - { - topic_ = bonds_topic; - bond_id_ = bond_id; - node_ = node; - event_ = event_client; - } - - /** - * @brief start the bond - * - */ - void start() - { - event_->info("[BondClient] creating bond to capabilities server"); - - bond_ = std::make_unique(topic_, bond_id_, node_, std::bind(&BondClient::on_broken, this), std::bind(&BondClient::on_formed, this)); - - bond_->setHeartbeatPeriod(0.10); - bond_->setHeartbeatTimeout(10.0); - bond_->start(); - } - - /** - * @brief stop the bond - * - */ - void stop() - { - event_->info("[BondClient] destroying bond to capabilities server"); - - if (bond_) - { - bond_.reset(); - } - } - - ~BondClient() - { - stop(); - } - -private: - /** - * @brief callback function for bond formed event - * - */ - void on_formed() - { - // log bond established event - event_->info("[BondClient] bond with capabilities server formed with id: " + bond_id_); - } - - /** - * @brief callback function for bond broken event - * - */ - void on_broken() - { - // log bond established event - event_->info("[BondClient] bond with capabilities server broken with id: " + bond_id_); - } - - /** Ros node pointer */ - rclcpp::Node::SharedPtr node_; - - /** Bond id string */ - std::string bond_id_; - - /** Bond topic to be published */ - std::string topic_; - - /** Heart beat bond with capabilities server */ - std::shared_ptr bond_; - - /** Event client for publishing events */ - std::shared_ptr event_; -}; \ No newline at end of file diff --git a/capabilities2_utils/include/capabilities2_utils/connection.hpp b/capabilities2_utils/include/capabilities2_utils/connection.hpp deleted file mode 100644 index 4d8acca..0000000 --- a/capabilities2_utils/include/capabilities2_utils/connection.hpp +++ /dev/null @@ -1,32 +0,0 @@ -#pragma once -#include -#include - -namespace capabilities2 -{ - enum connection_type_t - { - ON_START, - ON_SUCCESS, - ON_FAILURE, - ON_STOP - }; - - struct connection_t - { - std::string runner = ""; - std::string provider = ""; - tinyxml2::XMLElement* parameters = nullptr; - }; - - struct node_t { - connection_t source; - connection_t target_on_start; - connection_t target_on_stop; - connection_t target_on_success; - connection_t target_on_failure; - std::string connection_description; - int trigger_id = -1; - }; - -} // namespace capabilities2 diff --git a/capabilities2_utils/include/capabilities2_utils/event_types.hpp b/capabilities2_utils/include/capabilities2_utils/event_types.hpp deleted file mode 100644 index 9f756ef..0000000 --- a/capabilities2_utils/include/capabilities2_utils/event_types.hpp +++ /dev/null @@ -1,47 +0,0 @@ -#pragma once -#include - -namespace capabilities2 -{ -enum event_t -{ - IDLE, - STARTED, - STOPPED, - FAILED, - SUCCEEDED -}; - -/** - * @brief event definition - * - * Contains the informations about the event to be executed. It contains the interface, provider and parameters - */ -struct event -{ - std::string interface; - std::string provider; - std::string parameters; -}; - -/** - * @brief event options - * - * keeps track of events that are related to runner instances at various points of the - * plan - * @param event_id unique identifier for the event - * @param on_started information about the capability to execute on start - * @param on_success information about the capability to execute on success - * @param on_failure information about the capability to execute on failure - * @param on_stopped information about the capability to execute on stop - */ -struct event_opts -{ - int event_id = -1; - event on_started; - event on_success; - event on_failure; - event on_stopped; -}; - -} \ No newline at end of file diff --git a/capabilities2_utils/package.xml b/capabilities2_utils/package.xml deleted file mode 100644 index f6296bd..0000000 --- a/capabilities2_utils/package.xml +++ /dev/null @@ -1,23 +0,0 @@ - - - - capabilities2_utils - 0.0.0 - TODO: Package description - - Kalana Ratnayake - Kalana Ratnayake - - Kalana Ratnayake - - TODO: License declaration - - ament_cmake - - ament_lint_auto - ament_lint_common - - - ament_cmake - - From be5d50040c70166fd5627cda0a6020ac3c34f264 Mon Sep 17 00:00:00 2001 From: mik-p Date: Tue, 23 Sep 2025 16:06:48 +1000 Subject: [PATCH 04/37] more doc for bond cache class --- .../capabilities2_server/bond_cache.hpp | 34 +++++++++++++++++-- 1 file changed, 32 insertions(+), 2 deletions(-) diff --git a/capabilities2_server/include/capabilities2_server/bond_cache.hpp b/capabilities2_server/include/capabilities2_server/bond_cache.hpp index 4705371..8f106e4 100644 --- a/capabilities2_server/include/capabilities2_server/bond_cache.hpp +++ b/capabilities2_server/include/capabilities2_server/bond_cache.hpp @@ -13,16 +13,30 @@ namespace capabilities2_server /** * @brief bond cache class - * keep track of bonds established by clients and assosciated resources + * + * keep track of bonds established by clients and associated resources + * this is used to manage the lifecycle of capabilities based on client bonds + * a capability can have multiple bonds from different clients * */ class BondCache { public: + /** + * @brief Construct a Bond Cache with a specific bond topic + * + * @param bonds_topic + */ BondCache(const std::string& bonds_topic = "/capabilities/bond") : bonds_topic_(bonds_topic) { } + /** + * @brief Add a bond to the cache + * + * @param capability + * @param bond_id + */ void add_bond(const std::string& capability, const std::string& bond_id) { // if capability is not in cache, add it @@ -41,6 +55,13 @@ class BondCache } } + /** + * @brief Remove a bond id from the cache for all capabilities + * + * If a capability has no more bonds, it is removed from the cache + * + * @param bond_id + */ void remove_bond(const std::string& bond_id) { // remove bond id from all capability entries @@ -67,11 +88,20 @@ class BondCache } } + /** + * @brief Remove a bond id from a specific capability + * + * if the bond is used by another capability, it is not removed + * if the capability has no more bonds, it is removed from the cache + * + * @param capability + * @param bond_id + */ void remove_bond(const std::string& capability, const std::string& bond_id) { // remove bond id from capability entry auto it = std::find(bond_cache_[capability].begin(), bond_cache_[capability].end(), bond_id); - + if (it != bond_cache_[capability].end()) { bond_cache_[capability].erase(it); From e2ef538a078f1d792d24f667363301f387e6087a Mon Sep 17 00:00:00 2001 From: mik-p Date: Wed, 1 Oct 2025 10:47:55 +1000 Subject: [PATCH 05/37] small fixes, add docs --- .devcontainer/devcontainer.json | 11 ++--------- .../include/capabilities2_server/models/readme.md | 7 +++++++ .../launch/capabilities2_server.composed.launch.py | 6 +++++- .../launch/capabilities2_server.launch.py | 7 +++++-- 4 files changed, 19 insertions(+), 12 deletions(-) diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json index a99ace7..561d1a2 100644 --- a/.devcontainer/devcontainer.json +++ b/.devcontainer/devcontainer.json @@ -2,16 +2,9 @@ "name": "capabilities2 jazzy base", "dockerFile": "Dockerfile", "remoteUser": "ubuntu", - "containerEnv": { - "DISPLAY": "${localEnv:DISPLAY}" - }, "runArgs": [ - "--privileged", "--network=host" ], - "workspaceMount": "source=${localWorkspaceFolder},target=/home/ubuntu/colcon_ws/${localWorkspaceFolderBasename},type=bind", - "workspaceFolder": "/home/ubuntu/colcon_ws", - "mounts": [ - "source=${localEnv:HOME}${localEnv:USERPROFILE}/.bash_history,target=/home/ubuntu/.bash_history,type=bind" - ] + "workspaceMount": "source=${localWorkspaceFolder},target=/home/ubuntu/colcon_ws/src/${localWorkspaceFolderBasename},type=bind", + "workspaceFolder": "/home/ubuntu/colcon_ws" } diff --git a/capabilities2_server/include/capabilities2_server/models/readme.md b/capabilities2_server/include/capabilities2_server/models/readme.md index 66695ba..e625a63 100644 --- a/capabilities2_server/include/capabilities2_server/models/readme.md +++ b/capabilities2_server/include/capabilities2_server/models/readme.md @@ -16,6 +16,13 @@ This directory contains the data models used by the Capabilities2 server. |-------|-------------| | `predicates` | Represents predicates between capabilities | +### V3 new models (TODO)(Proposal) + +| Model | Description | +|-------|-------------| +| `running` | Represents active runners that are executing capabilities | +| `connections` | Represents connections between active runners | + ## Traits The models can implement various traits to provide additional functionality. The available traits are: diff --git a/capabilities2_server/launch/capabilities2_server.composed.launch.py b/capabilities2_server/launch/capabilities2_server.composed.launch.py index 498f824..894a05d 100644 --- a/capabilities2_server/launch/capabilities2_server.composed.launch.py +++ b/capabilities2_server/launch/capabilities2_server.composed.launch.py @@ -17,7 +17,11 @@ def generate_launch_description(): LaunchDescription: The launch description for capabilities2 server """ # load config file - server_config = os.path.join(get_package_share_directory('capabilities2_server'), 'config', 'capabilities.yaml') + server_config = os.path.join( + get_package_share_directory('capabilities2_server'), + 'config', + 'capabilities.yaml' + ) # create bridge composition capabilities = ComposableNodeContainer( diff --git a/capabilities2_server/launch/capabilities2_server.launch.py b/capabilities2_server/launch/capabilities2_server.launch.py index b112213..efcce28 100644 --- a/capabilities2_server/launch/capabilities2_server.launch.py +++ b/capabilities2_server/launch/capabilities2_server.launch.py @@ -17,8 +17,11 @@ def generate_launch_description(): LaunchDescription: The launch description for capabilities2 server """ # load config file - server_config = os.path.join(get_package_share_directory( - 'capabilities2_server'), 'config', 'capabilities.yaml') + server_config = os.path.join( + get_package_share_directory('capabilities2_server'), + 'config', + 'capabilities.yaml' + ) # create cap node capabilities2 = Node( From c9d5b4741ff8b5d8973a29aaff9eb2c95580bb52 Mon Sep 17 00:00:00 2001 From: mik-p Date: Wed, 1 Oct 2025 19:19:22 +1000 Subject: [PATCH 06/37] update msgs, adds connection and event codes --- capabilities2_msgs/CMakeLists.txt | 9 ++-- capabilities2_msgs/msg/CapabilityEvent.msg | 43 +++---------------- .../msg/CapabilityEventCode.msg | 15 +++++++ capabilities2_msgs/readme.md | 3 +- .../srv/ConfigureCapability.srv | 8 ---- capabilities2_msgs/srv/ConnectCapability.srv | 3 ++ 6 files changed, 31 insertions(+), 50 deletions(-) create mode 100644 capabilities2_msgs/msg/CapabilityEventCode.msg delete mode 100644 capabilities2_msgs/srv/ConfigureCapability.srv create mode 100644 capabilities2_msgs/srv/ConnectCapability.srv diff --git a/capabilities2_msgs/CMakeLists.txt b/capabilities2_msgs/CMakeLists.txt index af7c253..0971271 100644 --- a/capabilities2_msgs/CMakeLists.txt +++ b/capabilities2_msgs/CMakeLists.txt @@ -20,6 +20,7 @@ set(msg_files "msg/CapabilityCommand.msg" "msg/CapabilityConnection.msg" "msg/CapabilityEvent.msg" + "msg/CapabilityEventCode.msg" "msg/CapabilityEventStamped.msg" "msg/CapabilityResponse.msg" "msg/CapabilitySpec.msg" @@ -29,22 +30,22 @@ set(msg_files ) set(srv_files + "srv/ConnectCapability.srv" "srv/EstablishBond.srv" - "srv/GetCapabilitySpec.srv" "srv/FreeCapability.srv" + "srv/GetCapabilitySpec.srv" "srv/GetCapabilitySpecs.srv" "srv/GetInterfaces.srv" "srv/GetProviders.srv" "srv/GetRemappings.srv" "srv/GetRunningCapabilities.srv" "srv/GetSemanticInterfaces.srv" + "srv/Launch.srv" "srv/RegisterCapability.srv" "srv/StartCapability.srv" "srv/StopCapability.srv" - "srv/UseCapability.srv" - "srv/ConfigureCapability.srv" "srv/TriggerCapability.srv" - "srv/Launch.srv" + "srv/UseCapability.srv" ) set(action_files diff --git a/capabilities2_msgs/msg/CapabilityEvent.msg b/capabilities2_msgs/msg/CapabilityEvent.msg index 8862383..27c5e16 100644 --- a/capabilities2_msgs/msg/CapabilityEvent.msg +++ b/capabilities2_msgs/msg/CapabilityEvent.msg @@ -1,46 +1,15 @@ -# the capability event message type +# the capability event message + +# identifying name for the source that triggered this event +string trigger_id # Capabilities which this event pertains to (source and target) # also represents connections between capabilities # usually via runners capabilities2_msgs/CapabilityConnection connection -# PID of the related process -int32 pid - -# Thread id of the capability which this event pertains to -int8 thread_id - -# Node name which published this event -string node_id - -# Events available -uint8 IDLE=0 -# runner events -uint8 STARTED=1 # on_started -uint8 STOPPED=2 # on_stopped -uint8 FAILED=3 # on_failure -uint8 SUCCEEDED=4 # on_success -uint8 UNDEFINED=5 -# system events -uint8 SERVER_READY=6 # server ready -uint8 LAUNCHED=7 # process launched -uint8 TERMINATED=8 # process terminated - -# Related event -uint8 event +# the event code +capabilities2_msgs/CapabilityEventCode code # optional event message string description - -# Event level available -uint8 INFO=0 -uint8 DEBUG=1 -uint8 ERROR=2 -uint8 ERROR_ELEMENT=3 -uint8 RUNNER_DEFINE=4 -uint8 RUNNER_EVENT=5 - -# Related event level and optional message -uint8 level -string message diff --git a/capabilities2_msgs/msg/CapabilityEventCode.msg b/capabilities2_msgs/msg/CapabilityEventCode.msg new file mode 100644 index 0000000..5d5e918 --- /dev/null +++ b/capabilities2_msgs/msg/CapabilityEventCode.msg @@ -0,0 +1,15 @@ +# available events +uint8 IDLE=0 +# runner events +uint8 STARTED=1 # on_started +uint8 STOPPED=2 # on_stopped +uint8 FAILED=3 # on_failure +uint8 SUCCEEDED=4 # on_success +uint8 UNDEFINED=5 +# system events +uint8 SERVER_READY=6 # server ready +uint8 LAUNCHED=7 # process launched +uint8 TERMINATED=8 # process terminated + +# the event code +uint8 code diff --git a/capabilities2_msgs/readme.md b/capabilities2_msgs/readme.md index c39c164..ae55b71 100644 --- a/capabilities2_msgs/readme.md +++ b/capabilities2_msgs/readme.md @@ -12,6 +12,7 @@ This package contains [ROS2](https://index.ros.org/doc/ros2/) `messages`, `servi | `CapabilityCommand.msg` | A message type for a command to a robot. | | `CapabilityConnection.msg` | A message type for a connection between capabilities. | | `CapabilityEvent.msg` | A message type for an event related to a capability. | +| `CapabilityEventCode.msg` | event types | | `CapabilityEventStamped.msg` | A stamped version of event | | `CapabilityResponse.msg` | A message type for a response from a robot related to a capability. | | `CapabilitySpec.msg` | A message type for the specification of a capability. | @@ -39,7 +40,7 @@ This package contains [ROS2](https://index.ros.org/doc/ros2/) `messages`, `servi New in 0.1.3: -- `ConfigureCapability.srv` - A service type for configuring a capability. +- `ConnectCapability.srv` - A service type for connecting capabilities together. - `Launch.srv` - A service type for launching a launch file. - `TriggerCapability.srv` - A service type for triggering a capability. diff --git a/capabilities2_msgs/srv/ConfigureCapability.srv b/capabilities2_msgs/srv/ConfigureCapability.srv deleted file mode 100644 index b9e6905..0000000 --- a/capabilities2_msgs/srv/ConfigureCapability.srv +++ /dev/null @@ -1,8 +0,0 @@ -Capability source -Capability target_on_start -Capability target_on_stop -Capability target_on_success -Capability target_on_failure -string connection_description -int32 trigger_id ---- \ No newline at end of file diff --git a/capabilities2_msgs/srv/ConnectCapability.srv b/capabilities2_msgs/srv/ConnectCapability.srv new file mode 100644 index 0000000..0b1c264 --- /dev/null +++ b/capabilities2_msgs/srv/ConnectCapability.srv @@ -0,0 +1,3 @@ +# connect using an event +capabilities2_msgs/CapabilityEvent event +--- From 67d08f4475fc3691e644ee9a8cd665897c54a8c8 Mon Sep 17 00:00:00 2001 From: mik-p Date: Mon, 22 Dec 2025 01:08:59 +0000 Subject: [PATCH 07/37] adjust message defs to suit event api --- .../msg/CapabilityConnection.msg | 3 +++ capabilities2_msgs/msg/CapabilityEvent.msg | 5 +++-- .../msg/CapabilityEventCode.msg | 21 +++++++++++++------ capabilities2_msgs/srv/ConnectCapability.srv | 1 + capabilities2_msgs/srv/TriggerCapability.srv | 1 + 5 files changed, 23 insertions(+), 8 deletions(-) diff --git a/capabilities2_msgs/msg/CapabilityConnection.msg b/capabilities2_msgs/msg/CapabilityConnection.msg index 47b368e..11ca06f 100644 --- a/capabilities2_msgs/msg/CapabilityConnection.msg +++ b/capabilities2_msgs/msg/CapabilityConnection.msg @@ -1,6 +1,9 @@ # a connection between capabilities # usually used via an event +# the connection type through event code +capabilities2_msgs/CapabilityEventCode type + # Capability which this connection originates capabilities2_msgs/Capability source diff --git a/capabilities2_msgs/msg/CapabilityEvent.msg b/capabilities2_msgs/msg/CapabilityEvent.msg index 27c5e16..9501444 100644 --- a/capabilities2_msgs/msg/CapabilityEvent.msg +++ b/capabilities2_msgs/msg/CapabilityEvent.msg @@ -3,9 +3,10 @@ # identifying name for the source that triggered this event string trigger_id -# Capabilities which this event pertains to (source and target) -# also represents connections between capabilities +# event can pass over a connection +# could represent connection between capabilities # usually via runners +# Capabilities which this event pertains to (source and target) capabilities2_msgs/CapabilityConnection connection # the event code diff --git a/capabilities2_msgs/msg/CapabilityEventCode.msg b/capabilities2_msgs/msg/CapabilityEventCode.msg index 5d5e918..aec6fb3 100644 --- a/capabilities2_msgs/msg/CapabilityEventCode.msg +++ b/capabilities2_msgs/msg/CapabilityEventCode.msg @@ -1,15 +1,24 @@ # available events -uint8 IDLE=0 -# runner events +uint8 IDLE=0 # state and general +# runner state events uint8 STARTED=1 # on_started uint8 STOPPED=2 # on_stopped uint8 FAILED=3 # on_failure uint8 SUCCEEDED=4 # on_success -uint8 UNDEFINED=5 +# runner configuration events +uint8 CONNECTED=5 +uint8 DISCONNECTED=6 # system events -uint8 SERVER_READY=6 # server ready -uint8 LAUNCHED=7 # process launched -uint8 TERMINATED=8 # process terminated +uint8 SERVER_READY=7 # server ready +uint8 LAUNCHED=8 # process launched +uint8 TERMINATED=9 # process terminated + +uint8 ERROR=18 # defineable error event +# something out of scope went wrong code +uint8 UNDEFINED=19 + +# highest code +uint8 MAX_CODE=20 # the event code uint8 code diff --git a/capabilities2_msgs/srv/ConnectCapability.srv b/capabilities2_msgs/srv/ConnectCapability.srv index 0b1c264..c0088bc 100644 --- a/capabilities2_msgs/srv/ConnectCapability.srv +++ b/capabilities2_msgs/srv/ConnectCapability.srv @@ -1,3 +1,4 @@ +string bond_id # connect using an event capabilities2_msgs/CapabilityEvent event --- diff --git a/capabilities2_msgs/srv/TriggerCapability.srv b/capabilities2_msgs/srv/TriggerCapability.srv index b9160a7..f7eb34b 100644 --- a/capabilities2_msgs/srv/TriggerCapability.srv +++ b/capabilities2_msgs/srv/TriggerCapability.srv @@ -1,3 +1,4 @@ +string bond_id string capability string parameters --- From d2e77d18ba094f4ca8436d4f84adfcd4f25d8196 Mon Sep 17 00:00:00 2001 From: mik-p Date: Sun, 11 Jan 2026 00:53:32 +0000 Subject: [PATCH 08/37] doc, boilerplate for events --- capabilities2_events/.clang-format | 64 ++++++++++++++++++ capabilities2_events/package.xml | 27 ++++++++ capabilities2_events/readme.md | 101 +++++++++++++++++++++++++++++ capabilities2_runner/readme.md | 51 +++++++++++---- capabilities2_server/readme.md | 17 ++--- 5 files changed, 237 insertions(+), 23 deletions(-) create mode 100644 capabilities2_events/.clang-format create mode 100644 capabilities2_events/package.xml create mode 100644 capabilities2_events/readme.md diff --git a/capabilities2_events/.clang-format b/capabilities2_events/.clang-format new file mode 100644 index 0000000..d36804f --- /dev/null +++ b/capabilities2_events/.clang-format @@ -0,0 +1,64 @@ + +BasedOnStyle: Google +AccessModifierOffset: -2 +ConstructorInitializerIndentWidth: 2 +AlignEscapedNewlinesLeft: false +AlignTrailingComments: true +AllowAllParametersOfDeclarationOnNextLine: false +AllowShortIfStatementsOnASingleLine: false +AllowShortLoopsOnASingleLine: false +AllowShortFunctionsOnASingleLine: None +AlwaysBreakTemplateDeclarations: true +AlwaysBreakBeforeMultilineStrings: false +BreakBeforeBinaryOperators: false +BreakBeforeTernaryOperators: false +BreakConstructorInitializersBeforeComma: true +BinPackParameters: true +ColumnLimit: 120 +ConstructorInitializerAllOnOneLineOrOnePerLine: true +DerivePointerBinding: false +PointerBindsToType: true +ExperimentalAutoDetectBinPacking: false +IndentCaseLabels: true +MaxEmptyLinesToKeep: 1 +NamespaceIndentation: None +ObjCSpaceBeforeProtocolList: true +PenaltyBreakBeforeFirstCallParameter: 19 +PenaltyBreakComment: 60 +PenaltyBreakString: 1 +PenaltyBreakFirstLessLess: 1000 +PenaltyExcessCharacter: 1000 +PenaltyReturnTypeOnItsOwnLine: 90 +SpacesBeforeTrailingComments: 2 +Cpp11BracedListStyle: false +Standard: Auto +IndentWidth: 2 +TabWidth: 2 +UseTab: Never +IndentFunctionDeclarationAfterType: false +SpacesInParentheses: false +SpacesInAngles: false +SpaceInEmptyParentheses: false +SpacesInCStyleCastParentheses: false +SpaceAfterControlStatementKeyword: true +SpaceBeforeAssignmentOperators: true +ContinuationIndentWidth: 4 +SortIncludes: false +SpaceAfterCStyleCast: false + +# Configure each individual brace in BraceWrapping +BreakBeforeBraces: Custom + +# Control of individual brace wrapping cases +BraceWrapping: { + AfterClass: 'true' + AfterControlStatement: 'true' + AfterEnum : 'true' + AfterFunction : 'true' + AfterNamespace : 'true' + AfterStruct : 'true' + AfterUnion : 'true' + BeforeCatch : 'true' + BeforeElse : 'true' + IndentBraces : 'false' +} diff --git a/capabilities2_events/package.xml b/capabilities2_events/package.xml new file mode 100644 index 0000000..b412b93 --- /dev/null +++ b/capabilities2_events/package.xml @@ -0,0 +1,27 @@ + + + capabilities2_events + 0.2.0 + + Event subsystem for the capabilities2 framework + + + Michael Pritchard + Kalana Ratnayake + + Michael Pritchard + Kalana Ratnayake + + MIT + + ament_cmake + + rclcpp + capabilities2_msgs + tinyxml2_vendor + uuid + + + ament_cmake + + diff --git a/capabilities2_events/readme.md b/capabilities2_events/readme.md new file mode 100644 index 0000000..3914c08 --- /dev/null +++ b/capabilities2_events/readme.md @@ -0,0 +1,101 @@ +# Capabilities2 Events + +This package provides an event handling subsystem for the Capabilities2 framework. Events form the basis of inter-capability communication, allowing capabilities to respond to state changes in other capabilities. This facilitates the creation of complex behaviours through the composition of simpler capabilities. + +## features + +- Uses capability event messages +- helpers for integration with the capabilities2_runner, and capabilities2_server packages +- system for connecting capabilities together based on events. e.g. capability state transitions occur based on events from other capabilities + +## Model + +The event system is based around the concepts of nodes, connections, events. + +### Node + +Represents a capability that can emit events and has connections to other capabilities. + +| model data | description | +|---|---| +| id | unique identifier for the node | +| connections | a map of connections. The source i always *this* node | + +### Connection + +Represents the link between two nodes. + +| model data | description | +|---|---| +| type | the type of connection (e.g., ON_START, ON_STOP, ON_SUCCESS, ON_FAILURE) | +| source | the source capability that emits the event | +| target | the target capability to invoke when the event is emitted | + +### Event + +Represents a specific event that can be emitted by a node (e.g., STARTED, STOPPED, SUCCEEDED, FAILED) + +### event types + +State transitions are as defined in the event code message: + +``` +IDLE, // initial state +STARTED, // when capability is started +STOPPED, // when capability is stopped +// TRIGGERED, // when capability is TRIGGERED +FAILED, // when capability has FAILED +SUCCEEDED // when capability has SUCCEEDED +etc.. +``` + +## Flow + +1. A user establishes a bond with the capabilities2_server +2. The user connects capabilities together by specifying event connections (source capability, event type, target capability) +3. When a capability emits an event (e.g., STARTED), the event system checks for any connections matching that event type from the source capability +4. For each matching connection, the target capability is invoked accordingly (e.g., started, stopped, etc.) + +### Event Emission Triggering + +There are two ways events can be tracked for emission: + +1. the connection is for STARTED or STOPPED events - these are persistently tracked and become dependencies between capabilities +2. the connection is for SUCCEEDED or FAILED events - these are one-shot events that are emitted when a running capability is triggered to succeed or fail + +This works out to mean that a STARTED capability will implicitly start its dependent capabilities, and immediately run their trigger action. + +### Event trigger ID + +The event trigger ID is a unique identifier for the event connection. It also needs to account for multiple clients. It is constructed using URI format from the bond ID and the trigger ID specified by the user: + +``` +string connection_id = '/' +uuid bond_id -> provided on bond establishment +string trigger_id -> user specified id for the connection +event_id = connection_id = bond_id + '/' + trigger_id +``` + +### Notes + +- a single publisher is used for all event messages, this is at the top level (capabilities2_server) +- events are fired from runners + +#### To Do + +- [x] instantiate event publisher in server since publisher is defined there +- [x] store the event class in the api, since api owns runners +- [ ] then pass event system api object pointer to runners to allow firing events from runners, can be passed to event node parent class +- [ ] fix duplicate - event, trigger, runner - id - just use names? +- [ ] remove event_opts type usage + +- [x] implement event node class\ +- [x] inherit event node in runner base class +- [x] generalise event connections to use a map of event types to connection objects +- [x] add ability to disconnect events +- [x] add ability to list current event connections +- [ ] add ability to connect multiple events of same type to different targets + +- [ ] add ability to specify parameters for target capability on event connection + +- [ ] add ability to specify event connections in capability definition files diff --git a/capabilities2_runner/readme.md b/capabilities2_runner/readme.md index 439219a..152cd54 100644 --- a/capabilities2_runner/readme.md +++ b/capabilities2_runner/readme.md @@ -1,27 +1,48 @@ # capabilities2_runner plugin API -This package provides `runner` API for abstract provision of capabilities. Plugins extend the execution functionality of the `capabilities` system. The ROS1 implementation used launch files to start capabilities. The ROS2 implementation uses runners to start capabilities. This allows for more flexibility in how capabilities are started and stopped, or how they are managed, and operate. +This package provides the `runner` API for abstract provision of capabilities. Runner plugins extend the execution functionality of the `capabilities` system. The ROS1 implementation used launch files to start capabilities. The ROS2 implementation uses runners. This allows for more flexibility in how capabilities are started and stopped, or how they are managed, and operate. -## Runners +## Runner archetypes -The `capabilities2_runner` package provides runners that can be used to start capabilities and create capabilities. These runners are fully tested (test files are available): +The `capabilities2_runner` package provides runner patterns that can be used to specialise runners for capabilities and hence create capabilities. These runners are tested: -- `capabilities2_runner::RunnerBase` - The Base class for runners implementing the `Runner` interface which comprises of `start`, `stop` and `trigger` functionality. -- `capabilities2_runner::ActionRunner` - The Base runner class for capabilities that are implemented as ROS Actions. Overrides `stop` and `trigger` from RunnerBase. -- `capabilities2_runner::NoTriggerActionRunner` - A Base runner class that is also a derivative of Action Runner which disables trigger functionality. Useful for runners that has to start executing from the beginning. -- `capabilities2_runner::LaunchRunner` - Runner for capabilities that are implemented as launch files. -- `capabilities2_runner::DummyRunner` - A sample runner that can be used to test the functionality of capabilities server. +| Runner Type | Description | +|-------------|-------------| +| `capabilities2_runner::RunnerBase` | The Base class for runners implementing the `Runner` interface which consists of `start`, `stop` and `trigger` functionality. | +| `capabilities2_runner::ActionRunner` | The Base runner class for capabilities that are implemented as ROS Actions. Overrides `stop` and `trigger` from RunnerBase. | +| `capabilities2_runner::ServiceRunner` | The Base runner class for capabilities that are implemented as ROS Services. | +| `capabilities2_runner::TopicRunner` | The Base runner class for capabilities that are implemented as ROS Topics. | +| `capabilities2_runner::NoTriggerActionRunner` | A Base runner class that is also a derivative of Action Runner which disables trigger functionality. Useful for runners that start executing from the beginning. | + +## Standard Runners + +The `capabilities2_runner` package provides some standard runners. + +| Runner Type | Description | +|-------------|-------------| +| `capabilities2_runner::LaunchRunner` | Runner for capabilities that are implemented as launch files. | +| `capabilities2_runner::DummyRunner` | A sample runner that can be used to test the functionality of capabilities server. | + +## System Runners + +The `capabilities2_runner_system` package provides system-level runners that can be used to coordinate multiple capabilities through the events system. + +| Runner Type | Description | +|-------------|-------------| +| `capabilities2_runner_system::InputMultiplexAny` | A runner that multiplexes multiple input capabilities, allowing any of them to trigger the output. | +| `capabilities2_runner_system::InputMultiplexAll` | A runner that multiplexes multiple input capabilities, requiring all of them to be active to trigger the output. | ## Experimental Runners The `capabilities2_runner` package provides experimental runners that can be used to start capabilities. These runners are not fully tested and may not work as expected. The experimental runners are: -- `capabilities2_runner::EnCapRunner` - Base runner class that provides a capability action interface that encapsulates another action. -- `capabilities2_runner::MultiActionRunner` - Base runner class for capabilities that are implemented using multiple actions. +| Runner Type | Description | +|-------------|-------------| +| `capabilities2_runner::EnCapRunner` | Base runner class that provides a capability action interface that encapsulates another action. | +| `capabilities2_runner::MultiActionRunner` | Base runner class for capabilities that are implemented using multiple actions. | ## Runner Inheritance Diagram - Following inheritance diagram depicts the inheritance between the above presented *Runners* and *Experimental Runners*. ![inheritance diagram](../capabilities2_runner/docs/images/inheritance-diagram.png) @@ -58,7 +79,7 @@ namespace capabilities2_runner ### LaunchRunner -The `Launch Runner` inherits from the `capabilities2_runner::NoTriggerActionRunner` and is a special case. To instatiate this runner, provide a launch file path as the `runner` tag in the capability provider. +The `Launch Runner` inherits from the `capabilities2_runner::NoTriggerActionRunner` and is a special case. To instantiate this runner, provide a launch file path as the `runner` tag in the capability provider. ```yaml # provider ... @@ -72,7 +93,7 @@ runner: path/to/launch_file.launch.py ### Creating a a Custom runner -Runners can be created to perform capabilities. The runner can be specified in a capability provider as the `runner` tag: +The main idea is to allow users to create custom runners for their specific capabilities. Runners can be created to perform capabilities. The runner can be specified in a capability provider as the `runner` tag: ```yaml # provider ... @@ -103,3 +124,7 @@ An even more complex runner execution pattern is to start the runner, trigger it ### Start -> End (no Stop) The final runner execution pattern is to start the runner and then end it without stopping. This pattern represents a challenge for the runner API, as it is not clear when the runner should be stopped. ROS communications patterns including **Services** and **Actions** are like this type. + +## Trigger Parameter Format + +Runners can use parameters. These parameters are passed to the runner in the `trigger` function. For more information, see [Parameter Format](./docs/parameter_format.md). diff --git a/capabilities2_server/readme.md b/capabilities2_server/readme.md index 4b81b3a..9bbdf7f 100644 --- a/capabilities2_server/readme.md +++ b/capabilities2_server/readme.md @@ -46,25 +46,22 @@ The capabilities2 server exposes the following Service API (see [capabilities2_m | `~/get_capability_specs` | `GetCapabilitySpecs.srv` | Get all raw specifications in the capabilities server | | `~/get_running_capabilities` | `GetRunningCapabilities.srv`| Get the currently running capabilities | | `~/establish_bond` | `EstablishBond.srv` | Establish a bond with the capabilities server to use capabilities | -| `~/use_capability` | `UseCapability.srv` | Use a capability | -| `~/free_capability` | `FreeCapability.srv` | Free a capability (when done using it, when all users are done the capability is freed) | +| `~/use_capability` | `UseCapability.srv` | Use a capability - must be bonded | +| `~/free_capability` | `FreeCapability.srv` | Free a capability (when done using it, when all users are done the capability is freed) - must be bonded | | `~/start_capability` | `StartCapability.srv` | Start a capability (this is a forceful start, and ignores use and free logic) | | `~/stop_capability` | `StopCapability.srv` | Stop a capability (this is a forceful stop, and ignores use and free logic) | | `~/register_capability` | `RegisterCapability.srv` | Register a capability with the capabilities server | -| `~/trigger_capability` | `TriggerCapability.srv` | Trigger a capability | -| `~/configure_capability` | `ConfigureCapability.srv` | Configure a capability with `on_start`, `on_end`, `on_success`, `on_failure` events| - +| `~/trigger_capability` | `TriggerCapability.srv` | Trigger a capability - must be bonded | +| `~/connect_capability` | `ConnectCapability.srv` | Configure a capability with `on_start`, `on_end`, `on_success`, `on_failure` event connections - must be bonded | ### Topics The capabilities2 server exposes the following Topics API: | Topic | Message | Description | -| :--- | :--- | :--- | -| `~/events` | `GetInterfaces.srv` | Publish capability events | -| `~/bonds` | `GetSemanticInterfaces.srv` | Maintain bonds with capability users - [Bond API](https://wiki.ros.org/bond) | - -
+| :--- | :--- | :--- | +| `~/events` | `CapabilityEventStamped.msg` | Publish capability events | +| `~/bonds` | `bond/Status.msg` | Maintain bonds with capability users - [Bond API](https://wiki.ros.org/bond) | ## Additional Information From e563cc10623071f8ef3d55cca2e9038a3055f662 Mon Sep 17 00:00:00 2001 From: mik-p Date: Tue, 13 Jan 2026 03:17:56 +0000 Subject: [PATCH 09/37] change connect serve to use connection not event message --- capabilities2_msgs/srv/ConnectCapability.srv | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/capabilities2_msgs/srv/ConnectCapability.srv b/capabilities2_msgs/srv/ConnectCapability.srv index c0088bc..49884ea 100644 --- a/capabilities2_msgs/srv/ConnectCapability.srv +++ b/capabilities2_msgs/srv/ConnectCapability.srv @@ -1,4 +1,5 @@ -string bond_id -# connect using an event -capabilities2_msgs/CapabilityEvent event +string bond_id # user +string trigger_id # user specified unique (namespace-like) +# connect using a connection message +capabilities2_msgs/CapabilityConnection connection --- From bc4b3009de9d8b0245ad2560333bc06a5df5e091 Mon Sep 17 00:00:00 2001 From: mik-p Date: Wed, 14 Jan 2026 10:05:05 +0000 Subject: [PATCH 10/37] update build with event package, remove external util and log packages --- capabilities2_events/CMakeLists.txt | 37 +++++++++++++++++++ capabilities2_events/package.xml | 2 +- capabilities2_runner/CMakeLists.txt | 26 +++++++++---- capabilities2_runner/docs/parameter_format.md | 11 ++++++ capabilities2_runner/package.xml | 37 ++++++++++++++++--- capabilities2_runner/src/launch_runner.cpp | 2 +- .../src/multiplex_all_runner.cpp | 2 +- .../src/multiplex_any_runner.cpp | 2 +- capabilities2_server/CMakeLists.txt | 17 +++------ .../capabilities2_server/models/running.hpp | 6 +-- capabilities2_server/package.xml | 12 ++---- 11 files changed, 113 insertions(+), 41 deletions(-) create mode 100644 capabilities2_events/CMakeLists.txt create mode 100644 capabilities2_runner/docs/parameter_format.md diff --git a/capabilities2_events/CMakeLists.txt b/capabilities2_events/CMakeLists.txt new file mode 100644 index 0000000..6480098 --- /dev/null +++ b/capabilities2_events/CMakeLists.txt @@ -0,0 +1,37 @@ +cmake_minimum_required(VERSION 3.16) +project(capabilities2_events) + +# Default to C++17 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(capabilities2_msgs REQUIRED) +find_package(tinyxml2_vendor REQUIRED) +find_package(TinyXML2 REQUIRED) # provided by tinyxml2 upstream, or tinyxml2_vendor + +# find_package(PkgConfig) +# pkg_check_modules(UUID REQUIRED uuid) + +include_directories(include + ${TinyXML2_INCLUDE_DIRS} + # ${UUID_INCLUDE_DIRS} +) + +# install headers +install(DIRECTORY include/ + DESTINATION include +) + +ament_export_include_directories(include) +ament_export_dependencies(rclcpp capabilities2_msgs TinyXML2 tinyxml2_vendor) +# ament_export_libraries(${PROJECT_NAME}) + +ament_package() diff --git a/capabilities2_events/package.xml b/capabilities2_events/package.xml index b412b93..51126a3 100644 --- a/capabilities2_events/package.xml +++ b/capabilities2_events/package.xml @@ -19,7 +19,7 @@ rclcpp capabilities2_msgs tinyxml2_vendor - uuid + ament_cmake diff --git a/capabilities2_runner/CMakeLists.txt b/capabilities2_runner/CMakeLists.txt index 6021b30..7f356a7 100644 --- a/capabilities2_runner/CMakeLists.txt +++ b/capabilities2_runner/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 3.10) +cmake_minimum_required(VERSION 3.16) project(capabilities2_runner) # Default to C++17 @@ -15,9 +15,7 @@ find_package(rclcpp REQUIRED) find_package(rclcpp_action REQUIRED) find_package(pluginlib REQUIRED) find_package(capabilities2_msgs REQUIRED) -find_package(capabilities2_utils REQUIRED) -find_package(event_logger REQUIRED) -find_package(event_logger_msgs REQUIRED) +find_package(capabilities2_events REQUIRED) find_package(tinyxml2_vendor REQUIRED) find_package(TinyXML2 REQUIRED) # provided by tinyxml2 upstream, or tinyxml2_vendor @@ -26,7 +24,11 @@ include_directories( ) add_library(${PROJECT_NAME} SHARED - src/standard_runners.cpp + src/capability_get_runner.cpp + src/dummy_runner.cpp + src/launch_runner.cpp + src/multiplex_all_runner.cpp + src/multiplex_any_runner.cpp ) ament_target_dependencies(${PROJECT_NAME} @@ -34,9 +36,7 @@ ament_target_dependencies(${PROJECT_NAME} rclcpp_action pluginlib capabilities2_msgs - capabilities2_utils - event_logger - event_logger_msgs + capabilities2_events TinyXML2 ) @@ -53,6 +53,16 @@ install(DIRECTORY include/ DESTINATION include ) +# install capability interfaces and providers +install(DIRECTORY interfaces + DESTINATION share/${PROJECT_NAME} +) + +install(DIRECTORY providers + DESTINATION share/${PROJECT_NAME} +) + ament_export_include_directories(include) ament_export_libraries(${PROJECT_NAME}) + ament_package() diff --git a/capabilities2_runner/docs/parameter_format.md b/capabilities2_runner/docs/parameter_format.md new file mode 100644 index 0000000..dcb959a --- /dev/null +++ b/capabilities2_runner/docs/parameter_format.md @@ -0,0 +1,11 @@ +## Parameter Format + +# TODO + +Runners can use parameters. These parameters are passed to the runner in the `trigger` function. The parameter format for capabilities2 uses XML. A runner expects to receive a minimal XML structure which includes an ID attribute, for example: + +```xml + +``` + +The `id` is used to identify the runner instance and manage its execution. diff --git a/capabilities2_runner/package.xml b/capabilities2_runner/package.xml index f33f924..18c3089 100644 --- a/capabilities2_runner/package.xml +++ b/capabilities2_runner/package.xml @@ -1,16 +1,16 @@ capabilities2_runner - 0.0.1 + 0.2.0 - Package for capabilities2 runners, managed running different types of capabilities + Package for capabilities2 runners, plugin base classes, system-level and capability specific runner plugins Michael Pritchard - mik-p Kalana Ratnayake Michael Pritchard + Kalana Ratnayake MIT @@ -21,9 +21,7 @@ pluginlib std_msgs capabilities2_msgs - capabilities2_utils - event_logger - event_logger_msgs + capabilities2_events tinyxml2_vendor @@ -31,5 +29,32 @@ ament_cmake + + + + interfaces/InputMultiplexAllRunner.yaml + + + + providers/InputMultiplexAllRunner.yaml + + + + + interfaces/InputMultiplexAnyRunner.yaml + + + + providers/InputMultiplexAnyRunner.yaml + + + + + interfaces/CapabilityGetRunner.yaml + + + + providers/CapabilityGetRunner.yaml + diff --git a/capabilities2_runner/src/launch_runner.cpp b/capabilities2_runner/src/launch_runner.cpp index d001801..53e2844 100644 --- a/capabilities2_runner/src/launch_runner.cpp +++ b/capabilities2_runner/src/launch_runner.cpp @@ -1,6 +1,6 @@ #include #include -#include +#include // register runner plugins PLUGINLIB_EXPORT_CLASS(capabilities2_runner_capabilities::LaunchRunner, capabilities2_runner::RunnerBase) diff --git a/capabilities2_runner/src/multiplex_all_runner.cpp b/capabilities2_runner/src/multiplex_all_runner.cpp index 9896fc0..b58f771 100644 --- a/capabilities2_runner/src/multiplex_all_runner.cpp +++ b/capabilities2_runner/src/multiplex_all_runner.cpp @@ -1,6 +1,6 @@ #include #include -#include +#include // register runner plugins PLUGINLIB_EXPORT_CLASS(capabilities2_runner::InputMultiplexAllRunner, capabilities2_runner::RunnerBase); diff --git a/capabilities2_runner/src/multiplex_any_runner.cpp b/capabilities2_runner/src/multiplex_any_runner.cpp index 7b86da2..6058b28 100644 --- a/capabilities2_runner/src/multiplex_any_runner.cpp +++ b/capabilities2_runner/src/multiplex_any_runner.cpp @@ -1,6 +1,6 @@ #include #include -#include +#include // register runner plugins PLUGINLIB_EXPORT_CLASS(capabilities2_runner::InputMultiplexAllRunner, capabilities2_runner::RunnerBase); diff --git a/capabilities2_server/CMakeLists.txt b/capabilities2_server/CMakeLists.txt index bc9f429..b1210f8 100644 --- a/capabilities2_server/CMakeLists.txt +++ b/capabilities2_server/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 3.10) +cmake_minimum_required(VERSION 3.16) project(capabilities2_server) # Default to C++17 @@ -18,13 +18,10 @@ find_package(rclcpp_components REQUIRED) find_package(bondcpp REQUIRED) find_package(pluginlib REQUIRED) find_package(capabilities2_msgs REQUIRED) +find_package(capabilities2_events REQUIRED) find_package(capabilities2_runner REQUIRED) -find_package(capabilities2_utils REQUIRED) -find_package(event_logger REQUIRED) -find_package(event_logger_msgs REQUIRED) find_package(tinyxml2_vendor REQUIRED) find_package(TinyXML2 REQUIRED) # provided by tinyxml2 upstream, or tinyxml2_vendor -find_package(backward_ros REQUIRED) find_package(PkgConfig) pkg_check_modules(UUID REQUIRED uuid) @@ -62,10 +59,8 @@ ament_target_dependencies(${PROJECT_NAME}_comp pluginlib rclcpp_components capabilities2_msgs + capabilities2_events capabilities2_runner - capabilities2_utils - event_logger - event_logger_msgs TinyXML2 SQLite3 yaml-cpp @@ -87,7 +82,7 @@ install(TARGETS ${PROJECT_NAME}_comp ) ############################################################################ -# node implementation that compiles as a executable +# node implementation that compiles as a executable ############################################################################ add_executable(${PROJECT_NAME}_node @@ -106,10 +101,8 @@ ament_target_dependencies(${PROJECT_NAME}_node bondcpp pluginlib capabilities2_msgs + capabilities2_events capabilities2_runner - capabilities2_utils - event_logger - event_logger_msgs TinyXML2 SQLite3 yaml-cpp diff --git a/capabilities2_server/include/capabilities2_server/models/running.hpp b/capabilities2_server/include/capabilities2_server/models/running.hpp index 2ec7819..c68e1e3 100644 --- a/capabilities2_server/include/capabilities2_server/models/running.hpp +++ b/capabilities2_server/include/capabilities2_server/models/running.hpp @@ -23,10 +23,10 @@ struct capability_model_t * it is derived from interfaces, semantic interfaces, and providers. * */ -struct running_model_t +struct running_model_t : public capability_model_t { - std::string interface; - std::string provider; + // std::string interface; + // std::string provider; std::vector dependencies; std::string started_by; std::string pid; diff --git a/capabilities2_server/package.xml b/capabilities2_server/package.xml index 3efffc8..0afddcc 100644 --- a/capabilities2_server/package.xml +++ b/capabilities2_server/package.xml @@ -1,7 +1,7 @@ capabilities2_server - 0.0.1 + 0.2.0 Package that implements the capabilities2 service, a successor to capabilities @@ -11,6 +11,7 @@ Kalana Ratnayake Michael Pritchard + Kalana Ratnayake MIT @@ -22,24 +23,19 @@ pluginlib rclcpp_components capabilities2_msgs + capabilities2_events capabilities2_runner - capabilities2_utils - event_logger - event_logger_msgs tinyxml2_vendor yaml-cpp libsqlite3-dev uuid - backward_ros rclpy - launch_ros - capabilities2_launch_proxy + ament_cmake - From 07e10fa7351c5ca3a62db04785cc3c12726aa6dc Mon Sep 17 00:00:00 2001 From: mik-p Date: Wed, 28 Jan 2026 15:59:42 +1100 Subject: [PATCH 11/37] implement event system base classes --- .../capabilities2_events/event_base.hpp | 75 +++++ .../capabilities2_events/event_node.hpp | 259 ++++++++++++++++++ .../capabilities2_events/published_event.hpp | 45 +++ 3 files changed, 379 insertions(+) create mode 100644 capabilities2_events/include/capabilities2_events/event_base.hpp create mode 100644 capabilities2_events/include/capabilities2_events/event_node.hpp create mode 100644 capabilities2_events/include/capabilities2_events/published_event.hpp diff --git a/capabilities2_events/include/capabilities2_events/event_base.hpp b/capabilities2_events/include/capabilities2_events/event_base.hpp new file mode 100644 index 0000000..814e872 --- /dev/null +++ b/capabilities2_events/include/capabilities2_events/event_base.hpp @@ -0,0 +1,75 @@ +#pragma once + +#include +#include + +#include +#include +#include +#include + +namespace capabilities2_events +{ +/** + * @brief event exception + * + * Base class for event exceptions + * + */ +struct event_exception : public std::runtime_error +{ + using std::runtime_error::runtime_error; + + event_exception(const std::string& what) : std::runtime_error(what) + { + } + + virtual const char* what() const noexcept override + { + return std::runtime_error::what(); + } +}; + +/** + * @brief event base class + * + * Represents an event in the capabilities framework. + * An event can be triggered to notify other components + * about changes in running capability (runners) states. + */ +class EventBase +{ +public: + EventBase() + { + } + + ~EventBase(); + + /** + * @brief emit an event + * + * an event emission uses a parameterised callback + * which lets loosely-coupled capabilities propogate state changes + * when a source capability emits an event to a target capability + * + * @param trigger_id + * @param event_code + * @param source + * @param target + * @param callback + */ + virtual void emit(const std::string& trigger_id, const capabilities2_msgs::msg::CapabilityEventCode& event_code, + const capabilities2_msgs::msg::Capability& source, + const capabilities2_msgs::msg::Capability& target, + std::function callback) + { + // do callback + if (callback) + { + callback(target.capability, target.parameters); + } + } +}; + +} // namespace capabilities2_events diff --git a/capabilities2_events/include/capabilities2_events/event_node.hpp b/capabilities2_events/include/capabilities2_events/event_node.hpp new file mode 100644 index 0000000..6dc4ffe --- /dev/null +++ b/capabilities2_events/include/capabilities2_events/event_node.hpp @@ -0,0 +1,259 @@ +#pragma once + +#include +#include +#include +#include +#include + +#include + +#include + +#include +#include +#include + +namespace capabilities2_events +{ +/** + * @brief an event node is a source of an event + * + * events when emitted will trigger interactions with connected nodes + * + */ +class EventNode +{ +private: + /** + * @brief event pipe structure + * + * represents a connection from this event node to a target capability + * contains the callback to be invoked when the event is emitted + * + */ + struct EventPipe + { + // connection type + capabilities2_msgs::msg::CapabilityEventCode type; + + // connection target + capabilities2_msgs::msg::Capability target; + + // event callback + std::function callback; + }; + +public: + EventNode(std::shared_ptr event_emitter = nullptr) + : id_(rclcpp::create_uuid_string()), source_(), event_emitter_(event_emitter), connections_() + { + } + + virtual ~EventNode() = default; + + /** event handling */ + + /** + * @brief emit an event from this event node to all matching connections + * + * @param bond_id + * @param event_type + * @param parameters + */ + void emit_event(const std::string& bond_id, const capabilities2_msgs::msg::CapabilityEventCode& event_type, + const std::string& parameters) + { + // check if event emitter is set + if (!event_emitter_) + { + // No event emitter configured - silently skip + // This allows runners to work without event system if needed + return; + } + + // check each connection + for (const auto& [conn_id, connection] : connections_) + { + // extract bond_id from connection_id (format: "bond_id/trigger_id") + size_t slash_pos = conn_id.find('/'); + std::string conn_bond_id = (slash_pos != std::string::npos) ? conn_id.substr(0, slash_pos) : conn_id; + + // get targets for this event type and id namespace + if (connection.type.code == event_type.code && bond_id == conn_bond_id) + { + // parameterise target capability + // create a copy of target with updated parameters + capabilities2_msgs::msg::Capability target_with_params = connection.target; + target_with_params.parameters = parameters; + + // emit event via event api + // NOTE: callback invocation is handled by event api + // this allows the nodes to be decoupled + // nodes just keep track of connections + // modifying execution of other nodes is not owned by this class + // this lets the execution flow be made thread-safe + // in the scope where the thread is owned + event_emitter_->emit(conn_id, event_type, source_, target_with_params, connection.callback); + } + } + } + +protected: + /** + * @brief Set the source object + * + * @param src + */ + void set_source(const capabilities2_msgs::msg::Capability& src) + { + source_ = src; + } + + /** + * @brief event emitter for this event node + * + * allows late binding of event emitter (when a node is initialized) + * + * @param event_emitter + */ + void set_event_emitter(std::shared_ptr event_emitter) + { + event_emitter_ = event_emitter; + } + + /** + * @brief Add a new connection + * + * from this event node to a target capability with a given event type + * + * @param connection_id + * @param type + * @param target + * @param event_cb + * + * @throws event_exception if connection with given id already exists + */ + void add_connection(const std::string& connection_id, const capabilities2_msgs::msg::CapabilityEventCode& type, + const capabilities2_msgs::msg::Capability& target, + std::function event_cb) + { + // validate connection id + if (connections_.find(connection_id) != connections_.end()) + { + throw event_exception("connection with id: " + connection_id + " already exists"); + } + + // set up an event pipe + EventPipe conn; + conn.type = type; + conn.target = target; + conn.callback = event_cb; + + // add connection + connections_[connection_id] = conn; + } + + /** + * @brief Remove a connection by its id + * + * @param connection_id + */ + void remove_connection(const std::string& connection_id) + { + // remove connection + connections_.erase(connection_id); + } + + // helper members + + /** + * @brief clear all connections from this event node + */ + void clear_connections() + { + connections_.clear(); + } + + /** + * @brief List all connections from this event node + * + * @return std::vector + */ + std::vector list_connections() const + { + std::vector conns; + + for (const auto& [conn_id, connection] : connections_) + { + capabilities2_msgs::msg::CapabilityConnection c; + c.type = connection.type; + c.source = source_; + c.target = connection.target; + conns.push_back(c); + } + + return conns; + } + + /** + * @brief Check if a connection exists + * + * @param connection_id + * @return true + * @return false + */ + bool has_connection(const std::string& connection_id) const + { + return connections_.find(connection_id) != connections_.end(); + } + + /** + * @brief current connection count + * + * @return size_t + */ + size_t connection_count() const + { + return connections_.size(); + } + + /** + * @brief unique id of this event node + * + * @return const std::string& + */ + const std::string& get_id() const + { + return id_; + } + + /** + * @brief source capability of this event node + * + * @return const capabilities2_msgs::msg::Capability& + */ + const capabilities2_msgs::msg::Capability& get_source() const + { + return source_; + } + +private: + // unique id of the event node + // using uuid string + std::string id_; + + // source capability of this event node + capabilities2_msgs::msg::Capability source_; + + // event emitter + // used to emit events + // store as member to avoid passing around + std::shared_ptr event_emitter_; + + // connections from this event node to target capabilities + // Key: connection_id (format: "bond_id/trigger_id") + // Value: EventPipe with type, target, callback + std::map connections_; +}; +} // namespace capabilities2_events diff --git a/capabilities2_events/include/capabilities2_events/published_event.hpp b/capabilities2_events/include/capabilities2_events/published_event.hpp new file mode 100644 index 0000000..d428dde --- /dev/null +++ b/capabilities2_events/include/capabilities2_events/published_event.hpp @@ -0,0 +1,45 @@ +#pragma once + +#include +#include +#include +#include + +namespace capabilities2_events +{ + +class PublishedEvent : public EventBase +{ +public: + PublishedEvent(rclcpp::Publisher::SharedPtr event_pub) + : EventBase(), event_pub_(event_pub) + { + } + + ~PublishedEvent(); + + /** */ + void emit(const std::string& trigger_id, const capabilities2_msgs::msg::CapabilityEventCode& event_code, + const capabilities2_msgs::msg::Capability& source, const capabilities2_msgs::msg::Capability& target, + std::function callback) override + { + capabilities2_msgs::msg::CapabilityEventStamped event_msg; + event_msg.header.stamp = rclcpp::Clock().now(); + event_msg.trigger_id = trigger_id; + event_msg.event_code = event_code; + event_msg.source = source; + event_msg.target = target; + + // publish event + event_pub_->publish(event_msg); + + // call super + EventBase::emit(trigger_id, event_code, source, target, callback); + } + +private: + // event publisher + rclcpp::Publisher::SharedPtr event_pub_; +}; + +} // namespace capabilities2_events From aee2f027f0fb7724bd03859391d7c079c3d83c5f Mon Sep 17 00:00:00 2001 From: mik-p Date: Wed, 28 Jan 2026 16:07:04 +1100 Subject: [PATCH 12/37] add bond/cap exists method to bond cache --- .../capabilities2_server/bond_cache.hpp | 18 +++++++++++++++++- 1 file changed, 17 insertions(+), 1 deletion(-) diff --git a/capabilities2_server/include/capabilities2_server/bond_cache.hpp b/capabilities2_server/include/capabilities2_server/bond_cache.hpp index 8f106e4..faaf39c 100644 --- a/capabilities2_server/include/capabilities2_server/bond_cache.hpp +++ b/capabilities2_server/include/capabilities2_server/bond_cache.hpp @@ -141,12 +141,28 @@ class BondCache return bond_cache_[capability]; } - // exists in cache + // capability exists in cache + // a capability has at least one bond bool exists(const std::string& capability) { return bond_cache_.find(capability) != bond_cache_.end(); } + // bond id exists for a capability + // this bond id is associated with this capability + bool exists(const std::string& capability, const std::string& bond_id) + { + // capability exists guard + if (!exists(capability)) + { + return false; + } + + // check if bond id exists for capability + auto& bonds = bond_cache_[capability]; + return std::find(bonds.begin(), bonds.end(), bond_id) != bonds.end(); + } + // start a live bond void start(const std::string& bond_id, rclcpp::Node::SharedPtr node, std::function on_broken, std::function on_formed) From 0dca70b765c77ea323e0f3224c018b69e3a5cf29 Mon Sep 17 00:00:00 2001 From: mik-p Date: Fri, 30 Jan 2026 13:10:49 +1100 Subject: [PATCH 13/37] add addtional publishable events for server events, fix event class integration bugs, wip --- capabilities2_events/CMakeLists.txt | 12 +-- .../capabilities2_events/event_base.hpp | 32 +++++--- .../capabilities2_events/event_node.hpp | 16 ++-- .../capabilities2_events/published_event.hpp | 75 +++++++++++++++++-- capabilities2_events/package.xml | 2 +- capabilities2_events/readme.md | 8 +- 6 files changed, 107 insertions(+), 38 deletions(-) diff --git a/capabilities2_events/CMakeLists.txt b/capabilities2_events/CMakeLists.txt index 6480098..d9d5c41 100644 --- a/capabilities2_events/CMakeLists.txt +++ b/capabilities2_events/CMakeLists.txt @@ -14,16 +14,8 @@ endif() find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(capabilities2_msgs REQUIRED) -find_package(tinyxml2_vendor REQUIRED) -find_package(TinyXML2 REQUIRED) # provided by tinyxml2 upstream, or tinyxml2_vendor -# find_package(PkgConfig) -# pkg_check_modules(UUID REQUIRED uuid) - -include_directories(include - ${TinyXML2_INCLUDE_DIRS} - # ${UUID_INCLUDE_DIRS} -) +include_directories(include) # install headers install(DIRECTORY include/ @@ -31,7 +23,7 @@ install(DIRECTORY include/ ) ament_export_include_directories(include) -ament_export_dependencies(rclcpp capabilities2_msgs TinyXML2 tinyxml2_vendor) +ament_export_dependencies(rclcpp capabilities2_msgs) # ament_export_libraries(${PROJECT_NAME}) ament_package() diff --git a/capabilities2_events/include/capabilities2_events/event_base.hpp b/capabilities2_events/include/capabilities2_events/event_base.hpp index 814e872..bd71a53 100644 --- a/capabilities2_events/include/capabilities2_events/event_base.hpp +++ b/capabilities2_events/include/capabilities2_events/event_base.hpp @@ -2,6 +2,7 @@ #include #include +#include #include #include @@ -39,12 +40,18 @@ struct event_exception : public std::runtime_error */ class EventBase { +public: + typedef std::string capability_str_t; + typedef std::string parameter_str_t; + typedef std::string access_id_t; + typedef std::function event_callback_t; + public: EventBase() { } - ~EventBase(); + ~EventBase() = default; /** * @brief emit an event @@ -53,21 +60,24 @@ class EventBase * which lets loosely-coupled capabilities propogate state changes * when a source capability emits an event to a target capability * - * @param trigger_id - * @param event_code - * @param source - * @param target - * @param callback + * @param connection_id connection identifier (format: "bond_id/trigger_id") + * @param event_code type of event being emitted + * @param source source capability emitting the event + * @param target target capability receiving the event + * @param callback function to trigger target capability with (capability, parameters, bond_id) */ - virtual void emit(const std::string& trigger_id, const capabilities2_msgs::msg::CapabilityEventCode& event_code, + virtual void emit(const std::string& connection_id, const capabilities2_msgs::msg::CapabilityEventCode& event_code, const capabilities2_msgs::msg::Capability& source, - const capabilities2_msgs::msg::Capability& target, - std::function callback) + const capabilities2_msgs::msg::Capability& target, event_callback_t callback) { - // do callback + // extract bond_id from connection_id (format: "bond_id/trigger_id") + size_t slash_pos = connection_id.find('/'); + std::string bond_id = (slash_pos != std::string::npos) ? connection_id.substr(0, slash_pos) : connection_id; + + // do callback with bond_id for access control if (callback) { - callback(target.capability, target.parameters); + callback(target.capability, target.parameters, bond_id); } } }; diff --git a/capabilities2_events/include/capabilities2_events/event_node.hpp b/capabilities2_events/include/capabilities2_events/event_node.hpp index 6dc4ffe..50bddd2 100644 --- a/capabilities2_events/include/capabilities2_events/event_node.hpp +++ b/capabilities2_events/include/capabilities2_events/event_node.hpp @@ -4,7 +4,6 @@ #include #include #include -#include #include @@ -40,8 +39,8 @@ class EventNode // connection target capabilities2_msgs::msg::Capability target; - // event callback - std::function callback; + // event callback with signature: (capability, parameters, bond_id) + EventBase::event_callback_t callback; }; public: @@ -135,8 +134,7 @@ class EventNode * @throws event_exception if connection with given id already exists */ void add_connection(const std::string& connection_id, const capabilities2_msgs::msg::CapabilityEventCode& type, - const capabilities2_msgs::msg::Capability& target, - std::function event_cb) + const capabilities2_msgs::msg::Capability& target, EventBase::event_callback_t event_cb) { // validate connection id if (connections_.find(connection_id) != connections_.end()) @@ -238,6 +236,14 @@ class EventNode return source_; } + /** + * @brief event emitter is set on this node + */ + bool is_event_emitter_set() const + { + return event_emitter_ != nullptr; + } + private: // unique id of the event node // using uuid string diff --git a/capabilities2_events/include/capabilities2_events/published_event.hpp b/capabilities2_events/include/capabilities2_events/published_event.hpp index d428dde..631bda3 100644 --- a/capabilities2_events/include/capabilities2_events/published_event.hpp +++ b/capabilities2_events/include/capabilities2_events/published_event.hpp @@ -1,8 +1,8 @@ #pragma once -#include -#include #include + +#include #include namespace capabilities2_events @@ -16,13 +16,23 @@ class PublishedEvent : public EventBase { } - ~PublishedEvent(); + ~PublishedEvent() = default; - /** */ - void emit(const std::string& trigger_id, const capabilities2_msgs::msg::CapabilityEventCode& event_code, + /** + * @brief see EventBase::emit, specialised to publish events + */ + void emit(const std::string& connection_id, const capabilities2_msgs::msg::CapabilityEventCode& event_code, const capabilities2_msgs::msg::Capability& source, const capabilities2_msgs::msg::Capability& target, - std::function callback) override + EventBase::event_callback_t callback) override { + // split connection id to get trigger id + std::string trigger_id = connection_id; + size_t slash_pos = connection_id.find('/'); + if (slash_pos != std::string::npos) + { + trigger_id = connection_id.substr(slash_pos + 1); + } + capabilities2_msgs::msg::CapabilityEventStamped event_msg; event_msg.header.stamp = rclcpp::Clock().now(); event_msg.trigger_id = trigger_id; @@ -34,7 +44,58 @@ class PublishedEvent : public EventBase event_pub_->publish(event_msg); // call super - EventBase::emit(trigger_id, event_code, source, target, callback); + EventBase::emit(connection_id, event_code, source, target, callback); + } + + /** + * @brief on server ready event + * + * @param msg + */ + void on_server_ready(const std::string& msg) + { + capabilities2_msgs::msg::CapabilityEventStamped event_msg; + event_msg.header.stamp = rclcpp::Clock().now(); + event_msg.event_code.code = capabilities2_msgs::msg::CapabilityEventCode::SERVER_READY; + event_msg.source.capability = "capabilities2_server"; + event_msg.source.provider = "capabilities2_server"; + event_msg.description = msg; + + event_pub_->publish(event_msg); + } + + /** + * @brief on process launched event + * + * @param pid + */ + void on_process_launched(const std::string& pid) + { + capabilities2_msgs::msg::CapabilityEventStamped event_msg; + event_msg.header.stamp = rclcpp::Clock().now(); + event_msg.event_code.code = capabilities2_msgs::msg::CapabilityEventCode::PROCESS_LAUNCHED; + event_msg.source.capability = "capabilities2_server"; + event_msg.source.provider = "capabilities2_server"; + event_msg.description = "Process launched with PID: " + pid; + + event_pub_->publish(event_msg); + } + + /** + * @brief on process terminated event + * + * @param pid + */ + void on_process_terminated(const std::string& pid) + { + capabilities2_msgs::msg::CapabilityEventStamped event_msg; + event_msg.header.stamp = rclcpp::Clock().now(); + event_msg.event_code.code = capabilities2_msgs::msg::CapabilityEventCode::PROCESS_TERMINATED; + event_msg.source.capability = "capabilities2_server"; + event_msg.source.provider = "capabilities2_server"; + event_msg.description = "Process terminated with PID: " + pid; + + event_pub_->publish(event_msg); } private: diff --git a/capabilities2_events/package.xml b/capabilities2_events/package.xml index 51126a3..088ad7a 100644 --- a/capabilities2_events/package.xml +++ b/capabilities2_events/package.xml @@ -18,7 +18,7 @@ rclcpp capabilities2_msgs - tinyxml2_vendor + diff --git a/capabilities2_events/readme.md b/capabilities2_events/readme.md index 3914c08..6b85b7e 100644 --- a/capabilities2_events/readme.md +++ b/capabilities2_events/readme.md @@ -85,16 +85,16 @@ event_id = connection_id = bond_id + '/' + trigger_id - [x] instantiate event publisher in server since publisher is defined there - [x] store the event class in the api, since api owns runners -- [ ] then pass event system api object pointer to runners to allow firing events from runners, can be passed to event node parent class -- [ ] fix duplicate - event, trigger, runner - id - just use names? -- [ ] remove event_opts type usage +- [x] then pass event system api object pointer to runners to allow firing events from runners, can be passed to event node parent class +- [x] fix duplicate - event, trigger, runner - id - just use names? +- [x] remove event_opts type usage - [x] implement event node class\ - [x] inherit event node in runner base class - [x] generalise event connections to use a map of event types to connection objects - [x] add ability to disconnect events - [x] add ability to list current event connections -- [ ] add ability to connect multiple events of same type to different targets +- [x] add ability to connect multiple events of same type to different targets - [ ] add ability to specify parameters for target capability on event connection From dea6c94d28b83e0df5eca02019ba813d78b457b8 Mon Sep 17 00:00:00 2001 From: mik-p Date: Thu, 5 Feb 2026 14:15:40 +1100 Subject: [PATCH 14/37] add some system event emissions on runner system changes --- .../include/capabilities2_events/event_node.hpp | 8 ++++++++ capabilities2_msgs/msg/CapabilityEventCode.msg | 7 ++++--- 2 files changed, 12 insertions(+), 3 deletions(-) diff --git a/capabilities2_events/include/capabilities2_events/event_node.hpp b/capabilities2_events/include/capabilities2_events/event_node.hpp index 50bddd2..698d32e 100644 --- a/capabilities2_events/include/capabilities2_events/event_node.hpp +++ b/capabilities2_events/include/capabilities2_events/event_node.hpp @@ -150,6 +150,10 @@ class EventNode // add connection connections_[connection_id] = conn; + + // emit connected event + emit_event(connection_id.substr(0, connection_id.find('/')), + capabilities2_msgs::msg::CapabilityEventCode::CONNECTED, ""); } /** @@ -161,6 +165,10 @@ class EventNode { // remove connection connections_.erase(connection_id); + + // emit disconnected event + emit_event(connection_id.substr(0, connection_id.find('/')), + capabilities2_msgs::msg::CapabilityEventCode::DISCONNECTED, ""); } // helper members diff --git a/capabilities2_msgs/msg/CapabilityEventCode.msg b/capabilities2_msgs/msg/CapabilityEventCode.msg index aec6fb3..e8fb533 100644 --- a/capabilities2_msgs/msg/CapabilityEventCode.msg +++ b/capabilities2_msgs/msg/CapabilityEventCode.msg @@ -8,10 +8,11 @@ uint8 SUCCEEDED=4 # on_success # runner configuration events uint8 CONNECTED=5 uint8 DISCONNECTED=6 +uint8 TRIGGERED=7 # system events -uint8 SERVER_READY=7 # server ready -uint8 LAUNCHED=8 # process launched -uint8 TERMINATED=9 # process terminated +uint8 SERVER_READY=8 # server ready +uint8 LAUNCHED=9 # process launched +uint8 TERMINATED=10 # process terminated uint8 ERROR=18 # defineable error event # something out of scope went wrong code From b1bda2f3c53b855067789ec00ed44f3cd5ee117d Mon Sep 17 00:00:00 2001 From: mik-p Date: Fri, 6 Feb 2026 14:12:38 +1100 Subject: [PATCH 15/37] update logging pattern in cap server, update add connection service endpoint, implement event handling --- .../capabilities_server.hpp | 196 +++++++++--------- 1 file changed, 98 insertions(+), 98 deletions(-) diff --git a/capabilities2_server/include/capabilities2_server/capabilities_server.hpp b/capabilities2_server/include/capabilities2_server/capabilities_server.hpp index e5e5b30..c751415 100644 --- a/capabilities2_server/include/capabilities2_server/capabilities_server.hpp +++ b/capabilities2_server/include/capabilities2_server/capabilities_server.hpp @@ -11,19 +11,22 @@ #include #include -#include +// #include #include +#include + #include +#include #include #include #include #include #include #include -#include #include +#include #include #include #include @@ -32,11 +35,6 @@ #include #include -#include - -#include -#include - namespace capabilities2_server { @@ -83,13 +81,10 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI */ void initialize() { - // pubs - event_ = std::make_shared(shared_from_this(), "server", "/events"); - // params interface // loop rate declare_parameter("loop_rate", 5.0); - double loop_hz_ = get_parameter("loop_rate").as_double(); + loop_hz_ = get_parameter("loop_rate").as_double(); // db file declare_parameter("db_file", "~/.ros/capabilities/capabilities.sqlite3"); @@ -110,11 +105,10 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI if (rebuild) { // remove db file if it exists - event_->info("Removing the old capabilities database"); - + RCLCPP_INFO(get_logger(), "Rebuilding capabilities database"); if (std::remove(db_file.c_str()) != 0) { - event_->error("Error deleting database file " + db_file); + RCLCPP_ERROR(get_logger(), "Error deleting file %s", db_file.c_str()); } } @@ -122,17 +116,11 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI if (!std::filesystem::exists(db_path)) { // create db file path - event_->info("Creating capabilities database"); - std::filesystem::create_directories(db_path.parent_path()); } // init capabilities api - event_->info("Connecting Capabilities API with Database"); - - connect(db_file, event_); - - event_->info("Loading capabilities"); + connect(db_file, get_node_logging_interface()); // load capabilities from package paths for (const auto& package_path : package_paths) @@ -140,7 +128,13 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI load_capabilities(package_path); } - event_->info("Starting server interfaces"); + // pubs + event_pub_ = create_publisher("~/events", 10); + + // event publisher uses event subsystem + event_ = std::make_shared(event_pub_); + + // subs // services // establish bond @@ -173,10 +167,10 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI "~/use_capability", std::bind(&CapabilitiesServer::use_capability_cb, this, std::placeholders::_1, std::placeholders::_2)); - // configure capability - configure_capability_srv_ = create_service( - "~/configure_capability", - std::bind(&CapabilitiesServer::configure_capability_cb, this, std::placeholders::_1, std::placeholders::_2)); + // connect capabilities to each other + connect_capability_srv_ = create_service( + "~/connect_capability", + std::bind(&CapabilitiesServer::connect_capability_cb, this, std::placeholders::_1, std::placeholders::_2)); // register capability register_capability_srv_ = create_service( @@ -212,8 +206,11 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI "~/get_running_capabilities", std::bind(&CapabilitiesServer::get_running_capabilities_cb, this, std::placeholders::_1, std::placeholders::_2)); - // publish ready event - event_->info("capabilities server start up complete"); + // log ready + RCLCPP_INFO(get_logger(), "capabilities server started"); + + // fire/publish ready event + event_->on_server_ready("capabilities server start up complete"); } // service callbacks @@ -231,6 +228,10 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI void start_capability_cb(const std::shared_ptr req, std::shared_ptr res) { + // log warning about using unsafe start service + RCLCPP_WARN(get_logger(), "start_capability service is unsafe and intended for internal use only. " + "Use use_capability service with established bond instead."); + // try starting capability // TODO: handle errors start_capability(shared_from_this(), req->capability, req->preferred_provider); @@ -243,6 +244,10 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI void stop_capability_cb(const std::shared_ptr req, std::shared_ptr res) { + // log warning about using unsafe stop service + RCLCPP_WARN(get_logger(), "stop_capability service is unsafe and intended for internal use only. " + "Use free_capability service with established bond instead."); + // try stopping capability // TODO: handle errors stop_capability(req->capability); @@ -252,30 +257,46 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI } // trigger capability + // requires a bond to be established void trigger_capability_cb(const std::shared_ptr req, std::shared_ptr res) { - // try stopping capability + // make sure capability is not empty + if (req->capability.empty()) + { + RCLCPP_ERROR(get_logger(), "trigger_capability: capability is empty"); + return; + } + + // make sure bond id is provided + if (req->bond_id.empty()) + { + RCLCPP_ERROR(get_logger(), "trigger_capability: bond_id is empty"); + return; + } + + // try triggering capability // TODO: handle errors - trigger_capability(req->capability, req->parameters); + trigger_capability(req->capability, req->parameters, req->bond_id); // response is empty } // free capability + // requires bond to be established void free_capability_cb(const std::shared_ptr req, std::shared_ptr res) { // guard empty values if (req->capability.empty()) { - event_->error("free_capability: capability is empty"); + RCLCPP_ERROR(get_logger(), "free_capability: capability is empty"); return; } if (req->bond_id.empty()) { - event_->error("free_capability: bond_id is empty"); + RCLCPP_ERROR(get_logger(), "free_capability: bond_id is empty"); return; } @@ -286,25 +307,26 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI } // use capability + // requires bond to be established void use_capability_cb(const std::shared_ptr req, std::shared_ptr res) { // guard empty values if (req->capability.empty()) { - event_->error("use_capability: capability is empty"); + RCLCPP_ERROR(get_logger(), "use_capability: capability is empty"); return; } if (req->preferred_provider.empty()) { - event_->error("use_capability: preferred_provider is empty"); + RCLCPP_ERROR(get_logger(), "use_capability: preferred_provider is empty"); return; } if (req->bond_id.empty()) { - event_->error("use_capability: bond_id is empty"); + RCLCPP_ERROR(get_logger(), "use_capability: bond_id is empty"); return; } @@ -314,49 +336,28 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI // response is empty } - // use capability - void configure_capability_cb(const std::shared_ptr req, - std::shared_ptr res) + // FIXME: repair this to use new event subsystem + // connect capability + // requires bond to be established + void connect_capability_cb(const std::shared_ptr req, + std::shared_ptr res) { - capabilities2::event_opts event_options; - - event_options.event_id = req->trigger_id; - - event_options.on_started.interface = req->target_on_start.capability; - event_options.on_started.provider = req->target_on_start.provider; - event_options.on_started.parameters = req->target_on_start.parameters; - - event_->runner_define(req->source.capability, req->source.provider, req->target_on_start.capability, - req->target_on_start.provider, event_logger_msgs::msg::Event::STARTED, req->connection_description); - - event_options.on_failure.interface = req->target_on_failure.capability; - event_options.on_failure.provider = req->target_on_failure.provider; - event_options.on_failure.parameters = req->target_on_failure.parameters; - - event_->runner_define(req->source.capability, req->source.provider, req->target_on_failure.capability, - req->target_on_failure.provider, event_logger_msgs::msg::Event::FAILED, req->connection_description); - - event_options.on_success.interface = req->target_on_success.capability; - event_options.on_success.provider = req->target_on_success.provider; - event_options.on_success.parameters = req->target_on_success.parameters; - - event_->runner_define(req->source.capability, req->source.provider, req->target_on_success.capability, - req->target_on_success.provider, event_logger_msgs::msg::Event::SUCCEEDED, req->connection_description); - - event_options.on_stopped.interface = req->target_on_stop.capability; - event_options.on_stopped.provider = req->target_on_stop.provider; - event_options.on_stopped.parameters = req->target_on_stop.parameters; - - event_->runner_define(req->source.capability, req->source.provider, req->target_on_stop.capability, - req->target_on_stop.provider, event_logger_msgs::msg::Event::STOPPED, req->connection_description); + // need to have a bond established + if (req->bond_id.empty()) + { + RCLCPP_ERROR(get_logger(), "connect_capability: bond_id is empty"); + return; + } - event_->info("on_started : " + event_options.on_started.parameters); - event_->info("on_failure : " + event_options.on_failure.parameters); - event_->info("on_success : " + event_options.on_success.parameters); - event_->info("on_stopped : " + event_options.on_stopped.parameters); + // need to have a trigger id + if (req->trigger_id.empty()) + { + RCLCPP_ERROR(get_logger(), "connect_capability: trigger_id is empty"); + return; + } - // setup triggers between parameters - set_triggers(req->source.capability, event_options); + // api connect capability + connect_capability(req->trigger_id, req->connection, req->bond_id); // response is empty } @@ -368,7 +369,7 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI // guard empty values if (req->capability_spec.package.empty() || req->capability_spec.content.empty()) { - event_->error("register_capability: package or content is empty"); + RCLCPP_ERROR(get_logger(), "register_capability: package or content is empty"); return; } @@ -396,7 +397,7 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI { // set response // get semantic interfaces for given interface - res->semantic_interfaces = get_sematic_interfaces(req->interface); + res->semantic_interfaces = get_semantic_interfaces(req->interface); } // get providers @@ -418,7 +419,7 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI // if the spec is not empty set response if (capability_spec.content.empty()) { - event_->error("capability spec not found for resource: " + req->capability_spec); + RCLCPP_ERROR(get_logger(), "capability spec not found for resource: %s", req->capability_spec.c_str()); // BUG: throw error causes service to crash, this is a ROS2 bug // std::runtime_error("capability spec not found for resource: " + req->capability_spec); @@ -461,12 +462,12 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI private: void load_capabilities(const std::string& package_path) { - event_->debug("Loading capabilities from package path: " + package_path); + RCLCPP_DEBUG(get_logger(), "Loading capabilities from package path: %s", package_path.c_str()); // check if path exists if (!std::filesystem::exists(package_path)) { - event_->error("package path does not exist: " + package_path); + RCLCPP_ERROR(get_logger(), "package path does not exist: %s", package_path.c_str()); return; } @@ -493,7 +494,7 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI // load capabilities from packages in /opt/ros/*/share for (const auto& package : packages_root) { - event_->debug("Loading capabilities from package: " + package); + RCLCPP_DEBUG(get_logger(), "Loading capabilities from package: %s", package.c_str()); // package.xml exports std::string package_xml = package_path + "/" + package + "/package.xml"; @@ -501,7 +502,7 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI // check if package.xml exists if (!std::filesystem::exists(package_xml)) { - event_->error("package.xml does not exist: " + package_xml); + RCLCPP_ERROR(get_logger(), "package.xml does not exist: %s", package_xml.c_str()); continue; } @@ -514,7 +515,7 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI } catch (const std::runtime_error& e) { - event_->error("failed to parse package.xml file: " + std::string(e.what())); + RCLCPP_ERROR(get_logger(), "failed to parse package.xml file: %s", e.what()); continue; } @@ -523,7 +524,7 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI if (exports == nullptr) { - event_->error("No exports found in package.xml file: " + package_xml); + RCLCPP_ERROR(get_logger(), "No exports found in package.xml file: %s", package_xml.c_str()); continue; } @@ -554,13 +555,12 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI load_spec_content(package_path + "/" + package + "/" + spec_path, capability_spec); // add capability to db - event_->info("adding capability: " + package + "/" + spec_path); - + RCLCPP_INFO(get_logger(), "adding capability: %s/%s", package.c_str(), spec_path.c_str()); add_capability(capability_spec); } catch (const std::runtime_error& e) { - event_->error("failed to load spec file: " + std::string(e.what())); + RCLCPP_ERROR(get_logger(), "failed to load spec file: %s", e.what()); } } }; @@ -576,7 +576,7 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI // load capabilities from packages in workspace install folder for (const auto& package : packages_install) { - event_->debug("Loading capabilities from package: " + package); + RCLCPP_DEBUG(get_logger(), "Loading capabilities from package: %s", package.c_str()); // package.xml exports std::string package_xml = package_path + "/" + package + "/share/" + package + "/package.xml"; @@ -584,7 +584,7 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI // check if package.xml exists if (!std::filesystem::exists(package_xml)) { - event_->error("package.xml does not exist: " + package_xml); + RCLCPP_ERROR(get_logger(), "package.xml does not exist: %s", package_xml.c_str()); continue; } @@ -597,7 +597,7 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI } catch (const std::runtime_error& e) { - event_->error("failed to parse package.xml file: " + std::string(e.what())); + RCLCPP_ERROR(get_logger(), "failed to parse package.xml file: %s", e.what()); continue; } @@ -606,7 +606,7 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI if (exports == nullptr) { - event_->error("No exports found in package.xml file: " + package_xml); + RCLCPP_ERROR(get_logger(), "No exports found in package.xml file: %s", package_xml.c_str()); continue; } @@ -637,13 +637,13 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI load_spec_content(package_path + "/" + package + "/share/" + package + "/" + spec_path, capability_spec); // add capability to db - event_->info("adding capability: " + package + "/" + spec_path); + RCLCPP_INFO(get_logger(), "adding capability: %s/%s", package.c_str(), spec_path.c_str()); add_capability(capability_spec); } catch (const std::runtime_error& e) { - event_->error("failed to load spec file: " + std::string(e.what())); + RCLCPP_ERROR(get_logger(), "failed to load spec file: %s", e.what()); } } }; @@ -714,8 +714,8 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI double loop_hz_; // publishers - /** Event client for publishing events */ - std::shared_ptr event_; + // event publisher + rclcpp::Publisher::SharedPtr event_pub_; // services // establish bond @@ -726,9 +726,9 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI rclcpp::Service::SharedPtr stop_capability_srv_; // free capability rclcpp::Service::SharedPtr free_capability_srv_; - // free capability - rclcpp::Service::SharedPtr configure_capability_srv_; - // free capability + // connect capability + rclcpp::Service::SharedPtr connect_capability_srv_; + // trigger capability rclcpp::Service::SharedPtr trigger_capability_srv_; // use capability rclcpp::Service::SharedPtr use_capability_srv_; From dfd05ab143e878084bd8b1908683a36e664aae41 Mon Sep 17 00:00:00 2001 From: mik-p Date: Fri, 6 Feb 2026 14:20:17 +1100 Subject: [PATCH 16/37] update cap api, add connection service function, event subsystem integration, remove uuid dependency replace with rclcpp, simpler logging interface, update trigger service function --- capabilities2_server/CMakeLists.txt | 16 +- .../capabilities2_server/capabilities_api.hpp | 203 +++++++++++------- capabilities2_server/package.xml | 2 +- capabilities2_server/readme.md | 1 - 4 files changed, 133 insertions(+), 89 deletions(-) diff --git a/capabilities2_server/CMakeLists.txt b/capabilities2_server/CMakeLists.txt index b1210f8..577cd70 100644 --- a/capabilities2_server/CMakeLists.txt +++ b/capabilities2_server/CMakeLists.txt @@ -23,8 +23,8 @@ find_package(capabilities2_runner REQUIRED) find_package(tinyxml2_vendor REQUIRED) find_package(TinyXML2 REQUIRED) # provided by tinyxml2 upstream, or tinyxml2_vendor -find_package(PkgConfig) -pkg_check_modules(UUID REQUIRED uuid) +# find_package(PkgConfig) +# pkg_check_modules(UUID REQUIRED uuid) # Find SQLite3 find_package(SQLite3) @@ -35,7 +35,7 @@ find_package(yaml-cpp REQUIRED) include_directories(include ${SQLITE3_INCLUDE_DIRS} ${YAML_CPP_INCLUDE_DIRS} - ${UUID_INCLUDE_DIRS} + # ${UUID_INCLUDE_DIRS} ) ####################################################################### @@ -49,7 +49,7 @@ add_library(${PROJECT_NAME}_comp SHARED target_link_libraries(${PROJECT_NAME}_comp ${SQLITE3_LIBRARIES} ${YAML_CPP_LIBRARIES} - ${UUID_LIBRARIES} + # ${UUID_LIBRARIES} ) ament_target_dependencies(${PROJECT_NAME}_comp @@ -64,7 +64,7 @@ ament_target_dependencies(${PROJECT_NAME}_comp TinyXML2 SQLite3 yaml-cpp - UUID + # UUID ) rclcpp_components_register_node(${PROJECT_NAME}_comp @@ -90,7 +90,7 @@ add_executable(${PROJECT_NAME}_node ) target_link_libraries(${PROJECT_NAME}_node - ${UUID_LIBRARIES} + # ${UUID_LIBRARIES} ${SQLITE3_LIBRARIES} ${YAML_CPP_LIBRARIES} ) @@ -106,7 +106,7 @@ ament_target_dependencies(${PROJECT_NAME}_node TinyXML2 SQLite3 yaml-cpp - UUID + # UUID ) install(TARGETS ${PROJECT_NAME}_node @@ -121,7 +121,7 @@ target_link_libraries(test_capabilities_server ${PROJECT_NAME}_comp ${SQLITE3_LIBRARIES} ${YAML_CPP_LIBRARIES} - ${UUID_LIBRARIES} + # ${UUID_LIBRARIES} ) add_dependencies(test_capabilities_server ${PROJECT_NAME}_comp) diff --git a/capabilities2_server/include/capabilities2_server/capabilities_api.hpp b/capabilities2_server/include/capabilities2_server/capabilities_api.hpp index e7a246a..74e4815 100644 --- a/capabilities2_server/include/capabilities2_server/capabilities_api.hpp +++ b/capabilities2_server/include/capabilities2_server/capabilities_api.hpp @@ -7,7 +7,7 @@ #include #include -#include +// #include #include #include @@ -16,10 +16,8 @@ #include #include #include -#include -#include -#include +#include #include #include @@ -40,7 +38,7 @@ namespace capabilities2_server class CapabilitiesAPI { public: - CapabilitiesAPI() + CapabilitiesAPI() : event_(nullptr), cap_db_(nullptr), bond_cache_(), runner_cache_(), logging_(nullptr) { } @@ -48,19 +46,18 @@ class CapabilitiesAPI * @brief connect with the database file * * @param db_file file path of the database file - * @param event_client pointer to the event publishing interface + * @param logging pointer to the node logging interface for logging */ - void connect(const std::string& db_file, std::shared_ptr event_client) + void connect(const std::string& db_file, rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logging) { - event_ = event_client; - - runner_cache_.connect(event_client); + // set logger + logging_ = logging; // connect db cap_db_ = std::make_unique(db_file); // log - event_->info("Capabilities API connected to db: " + db_file); + RCLCPP_INFO(logging_->get_logger(), "Capabilities API connected to db: %s", db_file.c_str()); } /** @@ -84,7 +81,7 @@ class CapabilitiesAPI // go through the running model and start the necessary dependencies for (const auto& run : running.dependencies) { - event_->info("found dependency: " + run.interface); + RCLCPP_INFO(logging_->get_logger(), "found dependency: %s", run.interface.c_str()); // make an internal 'use' bond for the capability dependency bind_dependency(run.interface); @@ -105,40 +102,20 @@ class CapabilitiesAPI { runner_cache_.add_runner(node, capability, run_config); - event_->info("started capability: " + capability + " with provider: " + provider); + // log + RCLCPP_INFO(logging_->get_logger(), "started capability: %s with provider: %s", capability.c_str(), + provider.c_str()); return value and true; } catch (const capabilities2_runner::runner_exception& e) { - event_->error("could not start runner: " + std::string(e.what())); + RCLCPP_WARN(logging_->get_logger(), "could not start runner: %s", e.what()); return false; } } - /** - * @brief trigger a capability - * - * This is a new function for a capability provider (runner) that is already started but has - * additional parameters to be triggered. This function can be used externally. - * - * @param capability - * @param parameters - */ - void trigger_capability(const std::string& capability, const std::string& parameters) - { - // trigger the runner - try - { - runner_cache_.trigger_runner(capability, parameters); - } - catch (const capabilities2_runner::runner_exception& e) - { - event_->error("could not trigger runner: " + std::string(e.what())); - } - } - /** * @brief Stop a capability. Internal function only. Do not used this function externally. * @@ -150,7 +127,7 @@ class CapabilitiesAPI // this can happen if dependencies fail to resolve in the first place if (!runner_cache_.running(capability)) { - event_->error("could not get provider for: " + capability); + RCLCPP_ERROR(logging_->get_logger(), "could not get provider for: %s", capability.c_str()); return; } @@ -164,7 +141,7 @@ class CapabilitiesAPI // FIXME: this unrolls the dependency tree from the bottom up but should probably be top down for (const auto& run : running.dependencies) { - event_->info("freeing dependency: " + run.interface + "of : " + capability); + RCLCPP_INFO(logging_->get_logger(), "freeing dependency: %s of : %s", run.interface.c_str(), capability.c_str()); // remove the internal 'use' bond for the capability dependency unbind_dependency(run.interface); @@ -184,12 +161,12 @@ class CapabilitiesAPI } catch (const capabilities2_runner::runner_exception& e) { - event_->error("could not stop runner: " + std::string(e.what())); + RCLCPP_WARN(logging_->get_logger(), "could not stop runner: %s", e.what()); return; } // log - event_->info("stopped capability: " + capability); + RCLCPP_INFO(logging_->get_logger(), "stopped capability: %s", capability.c_str()); } /** @@ -208,12 +185,42 @@ class CapabilitiesAPI if (!bond_cache_.exists(capability)) { // stop the capability - event_->info("stopping freed capability: " + capability); - + RCLCPP_INFO(logging_->get_logger(), "stopping freed capability: %s", capability.c_str()); stop_capability(capability); } } + /** + * @brief trigger a capability + * + * This is a new function for a capability provider (runner) that is already started but has + * additional parameters to be triggered. This function can be used externally. + * + * @param capability + * @param parameters + * @param bond_id + */ + void trigger_capability(const std::string& capability, const std::string& parameters, const std::string& bond_id) + { + // validate bond + if (!bond_cache_.exists(capability, bond_id)) + { + RCLCPP_ERROR(logging_->get_logger(), "this bond: %s is not using capability: %s", bond_id.c_str(), + capability.c_str()); + return; + } + + // trigger the runner + try + { + runner_cache_.trigger_runner(capability, parameters, bond_id); + } + catch (const capabilities2_runner::runner_exception& e) + { + RCLCPP_ERROR(logging_->get_logger(), "could not trigger runner: %s", e.what()); + } + } + /** * @brief Use a capability. This will create a bond id for the requested instance and call start_capability * function can be used externally. @@ -236,29 +243,59 @@ class CapabilitiesAPI } /** - * @brief Set triggers for `on_success`, `on_failure`, `on_start`, `on_stop` events for a given capability + * @brief connect RUNNING capabilities together via event type * - * @param capability capability from where the events originate - * @param event_options event options for the capability + * TODO: Allow connections for not-running capabilities by storing connections in the db + * FIXME: implement new event subsystem + * Each running capability is a node that can be connected to other capabilities + * these connections emit events when the capability states change */ - void set_triggers(const std::string& capability, capabilities2::event_opts& event_options) + void connect_capability(const std::string& trigger_id, + const capabilities2_msgs::msg::CapabilityConnection& connection, const std::string& bond_id) { - try + // TODO: implement connection storage for non-running capabilities + // could also implicitly increment use counts for capabilities with this bond + // so that the capabilities are started and then connected + // could also increment use either way (running or not), which would allow tracking connections as 'uses' + // all of this would require freeing capabilities + + // validate bonds + if (!bond_cache_.exists(connection.source.capability, bond_id) || + !bond_cache_.exists(connection.target.capability, bond_id)) { - // log - event_->info("Setting triggers for capability: " + capability); + RCLCPP_ERROR(logging_->get_logger(), "no bond with id: %s exists for these capabilities", bond_id.c_str()); + return; + } + + // check if source and target capabilities are running + if (!runner_cache_.running(connection.source.capability) || !runner_cache_.running(connection.target.capability)) + { + RCLCPP_ERROR(logging_->get_logger(), "both source and target capabilities must be running to connect"); + return; + } - runner_cache_.set_runner_triggers(capability, event_options); + try + { + // new id for trigger - accounting for the bond + // lets use uri style id with bond and trigger id + const std::string connection_id = bond_id + '/' + trigger_id; + // need to pass event emitter to the runner cache so that it can be set in the runner + // this allows the runner to emit events on state changes + // default runner behaviour does not use event subsystem + runner_cache_.add_connection(connection.source.capability, connection_id, connection, event_); - event_->info("Successfully set triggers for capability: " + capability); + // log + RCLCPP_INFO(logging_->get_logger(), "set 'type %d' connection for runner: %s with id: %s", connection.type.code, + connection.source.capability.c_str(), connection_id.c_str()); } catch (const capabilities2_runner::runner_exception& e) { - event_->error("could not set triggers for the runner: " + std::string(e.what())); + RCLCPP_ERROR(logging_->get_logger(), "could not connect runners: %s", e.what()); } } // capability api + /** */ void add_capability(const capabilities2_msgs::msg::CapabilitySpec& spec) { // peak at the spec header @@ -272,7 +309,7 @@ class CapabilitiesAPI // exists guard if (cap_db_->exists(header.name)) { - event_->info(header.name + " interface already exists"); + RCLCPP_WARN(logging_->get_logger(), "interface already exists: %s", header.name.c_str()); return; } @@ -284,7 +321,7 @@ class CapabilitiesAPI } catch (const std::exception& e) { - event_->error("failed to convert spec to model: " + std::string(e.what())); + RCLCPP_ERROR(logging_->get_logger(), "failed to convert spec to model: %s", e.what()); return; } @@ -292,8 +329,8 @@ class CapabilitiesAPI model.header.name = spec.package + "/" + model.header.name; cap_db_->insert_interface(model); - event_->info("interface added to db: " + model.header.name); - + // log + RCLCPP_INFO(logging_->get_logger(), "interface added to db: %s", model.header.name.c_str()); return; } @@ -301,7 +338,7 @@ class CapabilitiesAPI { if (cap_db_->exists(header.name)) { - event_->info(header.name + " semantic interface already exists"); + RCLCPP_WARN(logging_->get_logger(), "semantic interface already exists: %s", header.name.c_str()); return; } @@ -313,7 +350,7 @@ class CapabilitiesAPI } catch (const std::exception& e) { - event_->error("failed to convert spec to model: " + std::string(e.what())); + RCLCPP_ERROR(logging_->get_logger(), "failed to convert spec to model: %s", e.what()); return; } @@ -321,8 +358,7 @@ class CapabilitiesAPI cap_db_->insert_semantic_interface(model); // log - event_->info("semantic interface added to db: " + model.header.name); - + RCLCPP_INFO(logging_->get_logger(), "semantic interface added to db: %s", model.header.name.c_str()); return; } @@ -330,7 +366,7 @@ class CapabilitiesAPI { if (cap_db_->exists(header.name)) { - event_->info(header.name + " provider already exists"); + RCLCPP_WARN(logging_->get_logger(), "provider already exists"); return; } @@ -342,7 +378,7 @@ class CapabilitiesAPI } catch (const std::exception& e) { - event_->error("failed to convert spec to model: " + std::string(e.what())); + RCLCPP_ERROR(logging_->get_logger(), "failed to convert spec to model: %s", e.what()); return; } @@ -350,12 +386,13 @@ class CapabilitiesAPI cap_db_->insert_provider(model); // log - event_->info("provider added to db: " + model.header.name); + RCLCPP_INFO(logging_->get_logger(), "provider added to db: %s", model.header.name.c_str()); return; } // couldn't parse unknown capability type - event_->error("unknown capability type: " + spec.type); + RCLCPP_ERROR(logging_->get_logger(), "unknown capability type: %s", spec.type.c_str()); + return; } // query api @@ -371,7 +408,7 @@ class CapabilitiesAPI return interfaces; } - std::vector get_sematic_interfaces(const std::string& interface) + std::vector get_semantic_interfaces(const std::string& interface) { std::vector semantic_interfaces; @@ -584,6 +621,8 @@ class CapabilitiesAPI return running_capabilities; } + /** */ + /** * @brief get pid of capability * @@ -614,13 +653,13 @@ class CapabilitiesAPI void on_bond_established(const std::string& bond_id) { // log bond established event - event_->info("bond established with id: " + bond_id); + RCLCPP_INFO(logging_->get_logger(), "bond established with id: %s", bond_id.c_str()); } void on_bond_broken(const std::string& bond_id) { // log warning - event_->error("bond broken for id: " + bond_id); + RCLCPP_WARN(logging_->get_logger(), "bond broken for id: %s", bond_id.c_str()); // get capabilities requested by the bond std::vector capabilities = bond_cache_.get_capabilities(bond_id); @@ -638,15 +677,16 @@ class CapabilitiesAPI * * @return const std::string */ - static const std::string gen_bond_id() - { - // create a new uuid for bond id - uuid_t uuid; - uuid_generate_random(uuid); - char uuid_str[40]; - uuid_unparse(uuid, uuid_str); - return std::string(uuid_str); - } + // DEPRECATED: use rclcpp UUID generator instead of uuid library for better compatibility and to remove dependency + // static const std::string gen_bond_id() + // { + // // create a new uuid for bond id + // uuid_t uuid; + // uuid_generate_random(uuid); + // char uuid_str[40]; + // uuid_unparse(uuid, uuid_str); + // return std::string(uuid_str); + // } private: // bind a dependency to an internal bond @@ -657,7 +697,8 @@ class CapabilitiesAPI void bind_dependency(const std::string& capability) { // create a new unique bond id - std::string bond_id = CapabilitiesAPI::gen_bond_id(); + // std::string bond_id = CapabilitiesAPI::gen_bond_id(); // DEPRECATED + std::string bond_id = rclcpp::create_uuid_string(); // add the bond id to the bond cache bond_cache_.add_bond(capability, bond_id); @@ -690,12 +731,16 @@ class CapabilitiesAPI } } +protected: + // event api + std::shared_ptr event_; + private: // db std::unique_ptr cap_db_; - // for events publishing - std::shared_ptr event_; + // for internal logging + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logging_; // caches BondCache bond_cache_; diff --git a/capabilities2_server/package.xml b/capabilities2_server/package.xml index 0afddcc..687fdad 100644 --- a/capabilities2_server/package.xml +++ b/capabilities2_server/package.xml @@ -30,7 +30,7 @@ yaml-cpp libsqlite3-dev - uuid + rclpy diff --git a/capabilities2_server/readme.md b/capabilities2_server/readme.md index 9bbdf7f..e51f9ff 100644 --- a/capabilities2_server/readme.md +++ b/capabilities2_server/readme.md @@ -26,7 +26,6 @@ The capabilities2 server depends on the following `bondcpp` ROS2 package. See th - `sqlite3` - `yaml-cpp` - `tinyxml2` -- `uuid` ## API From a3e58719e3632a87f4be9140361487d13d703157 Mon Sep 17 00:00:00 2001 From: mik-p Date: Fri, 6 Feb 2026 14:23:11 +1100 Subject: [PATCH 17/37] implement updated runner base cache access and runner connection function --- .../capabilities2_server/runner_cache.hpp | 78 ++++++++++--------- 1 file changed, 43 insertions(+), 35 deletions(-) diff --git a/capabilities2_server/include/capabilities2_server/runner_cache.hpp b/capabilities2_server/include/capabilities2_server/runner_cache.hpp index db7c9fe..8e1e686 100644 --- a/capabilities2_server/include/capabilities2_server/runner_cache.hpp +++ b/capabilities2_server/include/capabilities2_server/runner_cache.hpp @@ -4,12 +4,15 @@ #include #include #include -#include +#include +// #include #include #include + #include #include -#include +#include +#include namespace capabilities2_server { @@ -22,7 +25,7 @@ namespace capabilities2_server * * There are two main types of runners: * 1. launch file runner - * 2. action runner + * 2. runner - e.g. action, service, topic, etc. * */ class RunnerCache @@ -32,16 +35,6 @@ class RunnerCache { } - /** - * @brief connect with event interface - * - * @param event_client pointer to the event client - */ - void connect(std::shared_ptr event_client) - { - event_ = event_client; - } - /** * @brief Add a runner to the cache * @@ -86,37 +79,58 @@ class RunnerCache * xml parameters are used * * @param capability capability name to be loaded - * @param parameters parameters related to the runner in std::string form for compatibility accross various runners + * @param parameters parameters related to the runner in std::string form for compatibility across various runners + * @param bond_id unique identifier for the group on connections associated with this runner trigger */ - void trigger_runner(const std::string& capability, const std::string& parameters) + void trigger_runner(const std::string& capability, const std::string& parameters, const std::string& bond_id) { + // TODO: validate trigger id (DEPRECATED?) + // is the runner in the cache if (running(capability)) { - runner_cache_[capability]->trigger(parameters); + runner_cache_[capability]->trigger(parameters, bond_id); } else { - event_->error("Runner not found for capability: " + capability); throw capabilities2_runner::runner_exception("capability runner not found: " + capability); } } /** - * @brief Set triggers for `on_success`, `on_failure`, `on_start`, `on_stop` events - * + * @brief Add state change connection between runners via an event * - * @param capability capability from where the events originate - * @param on_started on_start event with capability and parameters - * @param on_failure on_failure event with capability and parameters - * @param on_success on_success event with capability and parameters - * @param on_stopped on_stop event with capability and parameters + * @param capability capability from where the event originates + * @param connection_id unique id for the connection + * @param connection connection options for the event */ - void set_runner_triggers(const std::string& capability, capabilities2::event_opts& event_options) + void add_connection(const std::string& capability, const std::string& connection_id, + const capabilities2_msgs::msg::CapabilityConnection& connection, + std::shared_ptr event_emitter = nullptr) { - runner_cache_[capability]->attach_events(event_options, - std::bind(&capabilities2_server::RunnerCache::trigger_runner, this, - std::placeholders::_1, std::placeholders::_2)); + // find the runner in the cache and if not found then throw an error + if (!running(capability)) + { + throw capabilities2_runner::runner_exception("capability runner not found: " + capability); + } + + // pass the event api to the runner + // for emitting events on state changes + runner_cache_[capability]->enable_events(event_emitter); + + try + { + // add connection to the runner + // callback signature: (capability, parameters, bond_id) + runner_cache_[capability]->add_connection(connection_id, connection.type, connection.target, + std::bind(&capabilities2_server::RunnerCache::trigger_runner, this, + std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3)); + } + catch (const capabilities2_events::event_exception& e) + { + throw capabilities2_runner::runner_exception(e.what()); + } } /** @@ -147,7 +161,7 @@ class RunnerCache // runner_cache_[capability].reset(); // remove the runner from map - // runner_cache_.erase(capability); + runner_cache_.erase(capability); } /** @@ -226,12 +240,6 @@ class RunnerCache // runner plugin loader pluginlib::ClassLoader runner_loader_; - - // for events publishing - std::shared_ptr event_; - - // event string - std::string event; }; } // namespace capabilities2_server From 3aa7bdf49a61b2a87ef506490427b464514eaaf4 Mon Sep 17 00:00:00 2001 From: mik-p Date: Fri, 6 Feb 2026 15:19:37 +1100 Subject: [PATCH 18/37] updating runner base with node inheritance and event emission. fixing thread model - WIP --- .../capabilities2_runner/runner_base.hpp | 490 +++++++++++------- 1 file changed, 300 insertions(+), 190 deletions(-) diff --git a/capabilities2_runner/include/capabilities2_runner/runner_base.hpp b/capabilities2_runner/include/capabilities2_runner/runner_base.hpp index b0e20ae..ed47228 100644 --- a/capabilities2_runner/include/capabilities2_runner/runner_base.hpp +++ b/capabilities2_runner/include/capabilities2_runner/runner_base.hpp @@ -8,11 +8,7 @@ #include #include -#include - -#include -#include -#include +#include namespace capabilities2_runner { @@ -66,17 +62,85 @@ struct runner_opts int input_count; }; -class RunnerBase +/** + * @brief base class for all runners + * + * Defines the runner plugin api + * inherits from EventNode to provide event emission + * + */ +class RunnerBase : public capabilities2_events::EventNode { public: - using Event = event_logger_msgs::msg::Event; - using EventType = capabilities2::event_t; + // static xml conversion + + /** + * @brief convert an XMLElement to std::string + * + * @param element XMLElement element to be converted + * @param parameters parameter to hold std::string + * + * @return `true` if element is not nullptr and conversion successful, `false` if element is nullptr + */ + static const std::string convert_to_string(tinyxml2::XMLElement* element) + { + if (element) + { + tinyxml2::XMLPrinter printer; + + element->Accept(&printer); + std::string parameters = printer.CStr(); + return parameters; + } + else + { + std::string parameters = ""; + return parameters; + } + } + + /** + * @brief convert an XMLElement to std::string + * + * @param element XMLElement element to be converted + * @param parameters parameter to hold std::string + * + * @return `true` if element is not nullptr and conversion successful, `false` if element is nullptr + */ + static tinyxml2::XMLElement* convert_to_xml(const std::string& parameters) + { + tinyxml2::XMLDocument doc; + + if (parameters != "") + { + doc.Parse(parameters.c_str()); + tinyxml2::XMLElement* element = doc.FirstChildElement(); + return element; + } + else + { + return nullptr; + } + } - RunnerBase() : run_config_() +public: + RunnerBase() : run_config_(), execution_should_stop_(false) { } - ~RunnerBase() = default; + ~RunnerBase() + { + std::scoped_lock lock(mutex_); + for (auto& [thread_id, exec_thread] : execution_thread_pool_) + { + if (exec_thread.joinable()) + { + RCLCPP_ERROR(node_->get_logger(), "Thread %s not cleaned up, detaching", thread_id.c_str()); + exec_thread.detach(); // don't block, but log the issue + } + } + execution_thread_pool_.clear(); + } /** runner plugin api */ @@ -87,39 +151,78 @@ class RunnerBase * * @param node shared pointer to the capabilities node. Allows to use ros node related functionalities * @param run_config runner configuration loaded from the yaml file - * @param print_ + * + * NOTE: must call init_base in derived class implementation + * NOTE: should call start event */ virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config) = 0; /** * @brief stop the runner * + * NOTE: should clean up threads + * NOTE: should call stop event */ virtual void stop() = 0; /** + * FIXME: implement new event subsystem + * * @brief Trigger the runner * * This method allows insertion of parameters in a runner after it has been initialized. it is an approach * to parameterise capabilities. Internally starts up RunnerBase::triggerExecution in a thread * * @param parameters pointer to tinyxml2::XMLElement that contains parameters + * @param bond_id unique identifier for the group of connections associated with this runner trigger event * */ - virtual void trigger(const std::string& parameters) + virtual void trigger(const std::string& parameters, const std::string& bond_id) { - // extract the unique id for the runner and use that as the thread id - tinyxml2::XMLElement * element = nullptr; - element = convert_to_xml(parameters); - element->QueryIntAttribute("id", &runner_id); + // TODO: verify parameter formatting (safe xml string or other format) + // TODO: minimum parameter set? + + // DEPRECATED: extract trigger id from parameters + // // extract the unique id for the runner and use that as the thread id + // tinyxml2::XMLElement* element = nullptr; + // element = convert_to_xml(parameters); + // if (!element) + // { + // // when this is empty it means that the trigger activation was performed + // // by a registered user + // // this is valid but we need to get a unique trigger id for the runner + // // to proceed safely + // RCLCPP_WARN(node_->get_logger(), "no trigger parameters provided"); + // } + + // std::string trigger_id = ""; + // element->QueryStringAttribute("id", &trigger_id); + + // parameters_[trigger_id] = element; - parameters_[runner_id] = element; + // create a thread id + std::string trigger_id = rclcpp::create_uuid_string(); + // namespace the thread id with bond id for later + // could list all threads related to a bond if needed + std::string thread_id = bond_id + "/" + trigger_id; - info_("received new parameters with event id : " + std::to_string(runner_id), runner_id); + // start execution thread + { + std::scoped_lock lock(mutex_); + // TODO: consider emitting on start event here + // emit_event(bond_id, capabilities2_msgs::msg::CapabilityEventCode::ON_STARTED, updated_on_started(parameters)); + execution_thread_pool_[thread_id] = std::thread(&RunnerBase::execution, this, parameters, bond_id); + } + + // emit trigger event + emit_event(bond_id, capabilities2_msgs::msg::CapabilityEventCode::TRIGGERED, ("")); - executionThreadPool[runner_id] = std::thread(&RunnerBase::execution, this, runner_id); + // BUG: thread management? - info_("started execution", runner_id); + // TODO: consider emitting on stop event here + // emit_event(bond_id, capabilities2_msgs::msg::CapabilityEventCode::ON_STOPPED, updated_on_stopped(parameters)); + + RCLCPP_DEBUG(node_->get_logger(), "started execution thread for runner id: %s", trigger_id.c_str()); } /** @@ -130,16 +233,36 @@ class RunnerBase */ void init_base(rclcpp::Node::SharedPtr node, const runner_opts& run_config) { + // store node connection source + capabilities2_msgs::msg::Capability source_capability; + source_capability.capability = run_config.interface; + source_capability.provider = run_config.provider; + set_source(source_capability); + // store node pointer and opts node_ = node; run_config_ = run_config; - current_inputs_ = 0; - runner_id = 0; + // FIXME: should be in derived class? + // current_inputs_ = 0; + // DEPRECATED: event node keeps a unique id + // runner_id = 0; + } - event_client_ = std::make_shared(node_, "runner", "/events"); + /** + * @brief enable events system for this runner + * + * @param events event emitter to be used by this runner + */ + void enable_events(std::shared_ptr events) + { + if (!is_event_emitter_set()) + { + set_event_emitter(events); + } } + // FIXME: implement new event subsystem /** * @brief attach events to the runner * @@ -148,14 +271,34 @@ class RunnerBase * * @return number of attached events */ - virtual void attach_events(capabilities2::event_opts& event_option, - std::function triggerFunction) - { - info_("accepted event options with ID : " + std::to_string(event_option.event_id)); + // MOVED TO: capabilities2_events::EventNode + // virtual void attach_events(capabilities2::event_opts& event_option, + // std::function triggerFunction) + // { + // // info_("accepted event options with ID : " + std::to_string(event_option.event_id)); - triggerFunction_ = triggerFunction; + // triggerFunction_ = triggerFunction; - events[event_option.event_id] = event_option; + // events[event_option.event_id] = event_option; + // } + /** + * @brief make EventNode::add_connection public method on runner api + * + * @param connection_id + * @param type + * @param target + * @param callback + */ + void add_connection(const std::string& connection_id, const capabilities2_msgs::msg::CapabilityEventCode& type, + const capabilities2_msgs::msg::Capability& target, + std::function callback) + { + // add connection to this runner to target capability + // this allows the runner to emit events on state changes + // to the target capability + // the connection ID format is: "bond_id/trigger_id" + // which allows event emission to extract bond_id for access control + EventNode::add_connection(connection_id, type, target, callback); } /** @@ -198,30 +341,64 @@ class RunnerBase return run_config_.pid; } + // FIXME: not used? /** - * @brief Get the execution status of runner. - * + * @brief Get the execution status of runner. + * * @return `true` if execution is complete, `false` otherwise. */ - const bool get_completion_status() const - { - return execution_complete_; - } + // const bool get_completion_status() const + // { + // return execution_complete_; + // } protected: /** * @brief Trigger process to be executed. * - * This method utilizes paramters set via the trigger() function - * - * @param id unique identifier for the runner id. used to track the correct - * triggers and subsequent events. + * This method utilizes parameters set via the trigger() function * + * @param parameters parameters for the execution + * + * NOTE: should call success and failure events appropriately */ - virtual void execution(int id) = 0; + virtual void execution(const std::string& parameters, const std::string& bond_id) = 0; + + /** */ + void stop_execution(std::chrono::milliseconds timeout = std::chrono::milliseconds(500)) + { + // signal listening threads to stop + execution_should_stop_ = true; + + // try join in a non-blocking way and log if threads are not cleaned up properly + { + std::scoped_lock lock(mutex_); + for (auto& [thread_id, exec_thread] : execution_thread_pool_) + { + if (exec_thread.joinable()) + { + if (exec_thread.try_join_for(timeout)) + { + RCLCPP_DEBUG(node_->get_logger(), "execution %s joined successfully", thread_id.c_str()); + } + else + { + RCLCPP_ERROR(node_->get_logger(), "execution %s did not stop in time, detaching", thread_id.c_str()); + exec_thread.detach(); // don't block, but log the issue + } + } + } + + // clear the thread pool + execution_thread_pool_.clear(); + } + } + + // FIXME: implement new event subsystem + // STATE CHANGE PARAMETER HELPERS /** - * @brief Update on_started event parameters with new data if avaible. + * @brief Update on_started event parameters with new data if available. * * This function is used to inject new data into the XMLElement containing * parameters related to the on_started trigger event @@ -237,7 +414,7 @@ class RunnerBase }; /** - * @brief Update on_stopped event parameters with new data if avaible. + * @brief Update on_stopped event parameters with new data if available. * * This function is used to inject new data into the XMLElement containing * parameters related to the on_stopped trigger event @@ -253,7 +430,7 @@ class RunnerBase }; /** - * @brief Update on_failure event parameters with new data if avaible. + * @brief Update on_failure event parameters with new data if available. * * This function is used to inject new data into the XMLElement containing * parameters related to the on_failure trigger event @@ -269,7 +446,7 @@ class RunnerBase }; /** - * @brief Update on_success event parameters with new data if avaible. + * @brief Update on_success event parameters with new data if available. * * This function is used to inject new data into the XMLElement containing * parameters related to the on_success trigger event @@ -284,50 +461,6 @@ class RunnerBase return parameters; }; - /** - * @brief convert an XMLElement to std::string - * - * @param element XMLElement element to be converted - * @param paramters parameter to hold std::string - * - * @return `true` if element is not nullptr and conversion successful, `false` if element is nullptr - */ - std::string convert_to_string(tinyxml2::XMLElement* element) - { - if (element) - { - element->Accept(&printer); - std::string parameters = printer.CStr(); - return parameters; - } - else - { - std::string parameters = ""; - return parameters; - } - } - - /** - * @brief convert an XMLElement to std::string - * - * @param element XMLElement element to be converted - * @param paramters parameter to hold std::string - * - * @return `true` if element is not nullptr and conversion successful, `false` if element is nullptr - */ - tinyxml2::XMLElement* convert_to_xml(const std::string& parameters) - { - if (parameters != "") - { - doc.Parse(parameters.c_str()); - tinyxml2::XMLElement* element = doc.FirstChildElement(); - return element; - } - else - { - return nullptr; - } - } // run config getters /** @@ -462,105 +595,64 @@ class RunnerBase } protected: - void info_(const std::string text, int thread_id = -1) - { - auto message = Event(); - - message.header.stamp = node_->now(); - message.origin_node = "runners"; - message.source.capability = run_config_.interface; - message.source.provider = run_config_.provider; - message.target.capability = ""; - message.target.provider = ""; - message.thread_id = thread_id; - message.type = Event::INFO; - message.content = text; - message.pid = -1; - message.event = Event::UNDEFINED; - - event_client_->publish(message); - } + // FIXME: implement new event subsystem + // STATE CHANGE HELPERS - void error_(const std::string text, int thread_id = -1) + /** + * @brief emit STARTED event + * + * @param bond_id + * @param parameters + */ + void emit_started(const std::string& bond_id, const std::string& parameters = "") { - auto message = Event(); - - message.header.stamp = node_->now(); - message.origin_node = "runners"; - message.source.capability = run_config_.interface; - message.source.provider = run_config_.provider; - message.target.capability = ""; - message.target.provider = ""; - message.thread_id = thread_id; - message.type = Event::ERROR; - message.content = text; - message.pid = -1; - message.event = Event::UNDEFINED; - - event_client_->publish(message); + capabilities2_msgs::msg::CapabilityEventCode event_type; + event_type.code = capabilities2_msgs::msg::CapabilityEventCode::STARTED; + emit_event(bond_id, event_type, parameters); } - void output_(const std::string text, const std::string element, int thread_id = -1) + /** + * @brief emit STOPPED event + * + * @param bond_id + * @param parameters + */ + void emit_stopped(const std::string& bond_id, const std::string& parameters = "") { - auto message = Event(); - - message.header.stamp = node_->now(); - message.origin_node = "runners"; - message.source.capability = run_config_.interface; - message.source.provider = run_config_.provider; - message.target.capability = ""; - message.target.provider = ""; - message.thread_id = thread_id; - message.type = Event::INFO; - message.content = text + " : " + element; - message.pid = -1; - message.event = Event::UNDEFINED; - - event_client_->publish(message); + capabilities2_msgs::msg::CapabilityEventCode event_type; + event_type.code = capabilities2_msgs::msg::CapabilityEventCode::STOPPED; + emit_event(bond_id, event_type, parameters); } - void event_(EventType event = EventType::IDLE, int thread_id = -1, const std::string& target_capability = "", - const std::string& target_provider = "") + /** + * @brief emit SUCCEEDED event + * + * @param bond_id + * @param parameters + */ + void emit_succeeded(const std::string& bond_id, const std::string& parameters = "") { - auto message = Event(); - - message.header.stamp = node_->now(); - message.origin_node = "runners"; - message.source.capability = run_config_.interface; - message.source.provider = run_config_.provider; - message.target.capability = target_capability; - message.target.provider = target_provider; - message.thread_id = thread_id; - message.type = Event::RUNNER_EVENT; - message.pid = -1; - - switch (event) - { - case EventType::IDLE: - message.event = Event::IDLE; - break; - case EventType::STARTED: - message.event = Event::STARTED; - break; - case EventType::STOPPED: - message.event = Event::STOPPED; - break; - case EventType::FAILED: - message.event = Event::FAILED; - break; - case EventType::SUCCEEDED: - message.event = Event::SUCCEEDED; - break; - default: - message.event = Event::UNDEFINED; - break; - } + capabilities2_msgs::msg::CapabilityEventCode event_type; + event_type.code = capabilities2_msgs::msg::CapabilityEventCode::SUCCEEDED; + emit_event(bond_id, event_type, parameters); + } - event_client_->publish(message); + /** + * @brief emit FAILED event + * + * @param bond_id + * @param parameters + */ + void emit_failed(const std::string& bond_id, const std::string& parameters = "") + { + capabilities2_msgs::msg::CapabilityEventCode event_type; + event_type.code = capabilities2_msgs::msg::CapabilityEventCode::FAILED; + emit_event(bond_id, event_type, parameters); } /** - * @brief shared pointer to the capabilities node. Allows to use ros node related functionalities + * @brief shared pointer to the capabilities node + * Allows to use ros node related functionalities */ rclcpp::Node::SharedPtr node_; @@ -572,67 +664,85 @@ class RunnerBase /** * @brief dictionary of events */ - std::map events; + // DEPRECATED: moved to capabilities2_events::EventNode + // std::map events; /** * @brief unique id for the runner */ - int runner_id; + // DEPRECATED: moved eventNode which uniquely identifies itself + // int runner_id; + // FIXME: should be moved to derived class /** - * @brief curent number of trigger signals received + * @brief current number of trigger signals received */ - int current_inputs_; + // int current_inputs_; + // FIXME: should be moved to derived class /** * @brief system runner completion tracking */ - bool execution_complete_; + // bool execution_complete_; /** * @brief pointer to XMLElement which contain parameters + * NOTE: std::string is thread/runner id */ - std::map parameters_; + // DEPRECATED: moved to execution function parameter since events track elsewhere + // just need to insert parameters into event emission during execution + // std::map parameters_; /** - * @brief dictionary of threads that executes the triggerExecution functionality + * @brief dictionary of threads that executes the execute function */ - std::map executionThreadPool; + std::map execution_thread_pool_; /** * @brief mutex for threadpool synchronisation. */ std::mutex mutex_; + /** + * @brief flag to signal execution threads to stop. + */ + std::atomic execution_should_stop_; + /** * @brief conditional variable for threadpool synchronisation. */ - std::condition_variable cv_; + // FIXME: move for derived class use + // std::condition_variable cv_; /** * @brief flag for threadpool synchronisation. */ - bool completed_; + // FIXME: move for derived class use + // bool completed_; + // FIXME: implement new event subsystem /** * @brief external function that triggers capability runners */ - std::function triggerFunction_; + // DEPRECATED: moved to capabilities2_events::EventNode + // std::function triggerFunction_; + // TODO: try make static helper class for xml conversion /** * @brief XMLElement that is used to convert xml strings to std::string */ - tinyxml2::XMLPrinter printer; + // tinyxml2::XMLPrinter printer; /** * @brief XMLElement that is used to convert std::string to xml strings */ - tinyxml2::XMLDocument doc; + // tinyxml2::XMLDocument doc; /** - * @brief client for publishing events + * @brief client api for event emission */ - std::shared_ptr event_client_; + // DEPRECATED: moved to capabilities2_events::EventNode + // std::shared_ptr event_; }; } // namespace capabilities2_runner From 3bbe89c90b085348382b53dc2c37aca9ed6f8b26 Mon Sep 17 00:00:00 2001 From: mik-p Date: Fri, 6 Feb 2026 16:08:50 +1100 Subject: [PATCH 19/37] stop exec in desctructor not duplicate --- .../include/capabilities2_runner/runner_base.hpp | 12 ++---------- 1 file changed, 2 insertions(+), 10 deletions(-) diff --git a/capabilities2_runner/include/capabilities2_runner/runner_base.hpp b/capabilities2_runner/include/capabilities2_runner/runner_base.hpp index ed47228..1ab4c38 100644 --- a/capabilities2_runner/include/capabilities2_runner/runner_base.hpp +++ b/capabilities2_runner/include/capabilities2_runner/runner_base.hpp @@ -130,16 +130,8 @@ class RunnerBase : public capabilities2_events::EventNode ~RunnerBase() { - std::scoped_lock lock(mutex_); - for (auto& [thread_id, exec_thread] : execution_thread_pool_) - { - if (exec_thread.joinable()) - { - RCLCPP_ERROR(node_->get_logger(), "Thread %s not cleaned up, detaching", thread_id.c_str()); - exec_thread.detach(); // don't block, but log the issue - } - } - execution_thread_pool_.clear(); + // clean up threads on destruction + stop_execution(std::chrono::milliseconds(500)); } /** runner plugin api */ From b210da7a6661c3f3279ed69f4d7440b9dc3b4921 Mon Sep 17 00:00:00 2001 From: mik-p Date: Mon, 9 Feb 2026 02:36:31 +0000 Subject: [PATCH 20/37] fixes to func sigs to compile, add gen uuid function and lib compile in cmake to support uuid gen in higher libraries --- capabilities2_events/CMakeLists.txt | 29 +++++++++++++++++-- .../capabilities2_events/event_base.hpp | 2 +- .../capabilities2_events/event_node.hpp | 8 ++--- .../capabilities2_events/published_event.hpp | 4 +-- .../capabilities2_events/uuid_generator.hpp | 23 +++++++++++++++ capabilities2_events/package.xml | 3 +- capabilities2_events/src/uuid_generator.cpp | 20 +++++++++++++ 7 files changed, 78 insertions(+), 11 deletions(-) create mode 100644 capabilities2_events/include/capabilities2_events/uuid_generator.hpp create mode 100644 capabilities2_events/src/uuid_generator.cpp diff --git a/capabilities2_events/CMakeLists.txt b/capabilities2_events/CMakeLists.txt index d9d5c41..fb5ead3 100644 --- a/capabilities2_events/CMakeLists.txt +++ b/capabilities2_events/CMakeLists.txt @@ -15,15 +15,40 @@ find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(capabilities2_msgs REQUIRED) -include_directories(include) +# uuid +find_package(PkgConfig) +pkg_check_modules(UUID REQUIRED uuid) + +include_directories(include ${UUID_INCLUDE_DIRS}) + +# build library +add_library(${PROJECT_NAME} SHARED + src/uuid_generator.cpp +) + +target_link_libraries(${PROJECT_NAME} ${UUID_LIBRARIES}) + +ament_target_dependencies(${PROJECT_NAME} + rclcpp + capabilities2_msgs + UUID +) # install headers install(DIRECTORY include/ DESTINATION include ) +# install library +install(TARGETS ${PROJECT_NAME} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + ament_export_include_directories(include) ament_export_dependencies(rclcpp capabilities2_msgs) -# ament_export_libraries(${PROJECT_NAME}) +ament_export_libraries(${PROJECT_NAME}) ament_package() diff --git a/capabilities2_events/include/capabilities2_events/event_base.hpp b/capabilities2_events/include/capabilities2_events/event_base.hpp index bd71a53..4cd213e 100644 --- a/capabilities2_events/include/capabilities2_events/event_base.hpp +++ b/capabilities2_events/include/capabilities2_events/event_base.hpp @@ -66,7 +66,7 @@ class EventBase * @param target target capability receiving the event * @param callback function to trigger target capability with (capability, parameters, bond_id) */ - virtual void emit(const std::string& connection_id, const capabilities2_msgs::msg::CapabilityEventCode& event_code, + virtual void emit(const std::string& connection_id, const uint8_t& event_code, const capabilities2_msgs::msg::Capability& source, const capabilities2_msgs::msg::Capability& target, event_callback_t callback) { diff --git a/capabilities2_events/include/capabilities2_events/event_node.hpp b/capabilities2_events/include/capabilities2_events/event_node.hpp index 698d32e..a101b1e 100644 --- a/capabilities2_events/include/capabilities2_events/event_node.hpp +++ b/capabilities2_events/include/capabilities2_events/event_node.hpp @@ -7,6 +7,7 @@ #include +#include #include #include @@ -45,7 +46,7 @@ class EventNode public: EventNode(std::shared_ptr event_emitter = nullptr) - : id_(rclcpp::create_uuid_string()), source_(), event_emitter_(event_emitter), connections_() + : id_(UUIDGenerator::gen_uuid_str()), source_(), event_emitter_(event_emitter), connections_() { } @@ -60,8 +61,7 @@ class EventNode * @param event_type * @param parameters */ - void emit_event(const std::string& bond_id, const capabilities2_msgs::msg::CapabilityEventCode& event_type, - const std::string& parameters) + void emit_event(const std::string& bond_id, const uint8_t& event_type, const std::string& parameters) { // check if event emitter is set if (!event_emitter_) @@ -79,7 +79,7 @@ class EventNode std::string conn_bond_id = (slash_pos != std::string::npos) ? conn_id.substr(0, slash_pos) : conn_id; // get targets for this event type and id namespace - if (connection.type.code == event_type.code && bond_id == conn_bond_id) + if (connection.type.code == event_type && bond_id == conn_bond_id) { // parameterise target capability // create a copy of target with updated parameters diff --git a/capabilities2_events/include/capabilities2_events/published_event.hpp b/capabilities2_events/include/capabilities2_events/published_event.hpp index 631bda3..83f181d 100644 --- a/capabilities2_events/include/capabilities2_events/published_event.hpp +++ b/capabilities2_events/include/capabilities2_events/published_event.hpp @@ -21,7 +21,7 @@ class PublishedEvent : public EventBase /** * @brief see EventBase::emit, specialised to publish events */ - void emit(const std::string& connection_id, const capabilities2_msgs::msg::CapabilityEventCode& event_code, + void emit(const std::string& connection_id, const uint8_t& event_code, const capabilities2_msgs::msg::Capability& source, const capabilities2_msgs::msg::Capability& target, EventBase::event_callback_t callback) override { @@ -36,7 +36,7 @@ class PublishedEvent : public EventBase capabilities2_msgs::msg::CapabilityEventStamped event_msg; event_msg.header.stamp = rclcpp::Clock().now(); event_msg.trigger_id = trigger_id; - event_msg.event_code = event_code; + event_msg.event_code.code = event_code; event_msg.source = source; event_msg.target = target; diff --git a/capabilities2_events/include/capabilities2_events/uuid_generator.hpp b/capabilities2_events/include/capabilities2_events/uuid_generator.hpp new file mode 100644 index 0000000..db5b703 --- /dev/null +++ b/capabilities2_events/include/capabilities2_events/uuid_generator.hpp @@ -0,0 +1,23 @@ +#pragma once + +#include + +namespace capabilities2_events +{ +class UUIDGenerator +{ +public: + /** + * @brief generate a uuid string + * + * @return const std::string + */ + static const std::string gen_uuid_str(); + +private: + // delete constructor and assignment operator to prevent instantiation + UUIDGenerator() = delete; + UUIDGenerator(const UUIDGenerator&) = delete; + UUIDGenerator& operator=(const UUIDGenerator&) = delete; +}; +} // namespace capabilities2_events diff --git a/capabilities2_events/package.xml b/capabilities2_events/package.xml index 088ad7a..e26ef65 100644 --- a/capabilities2_events/package.xml +++ b/capabilities2_events/package.xml @@ -18,8 +18,7 @@ rclcpp capabilities2_msgs - - + uuid ament_cmake diff --git a/capabilities2_events/src/uuid_generator.cpp b/capabilities2_events/src/uuid_generator.cpp new file mode 100644 index 0000000..85f2e21 --- /dev/null +++ b/capabilities2_events/src/uuid_generator.cpp @@ -0,0 +1,20 @@ +#include +#include + +namespace capabilities2_events +{ +/** + * @brief generate a uuid string + * + * @return const std::string + */ +const std::string UUIDGenerator::gen_uuid_str() +{ + // generate a new uuid + uuid_t uuid; + uuid_generate_random(uuid); + char uuid_str[40]; + uuid_unparse(uuid, uuid_str); + return std::string(uuid_str); +} +} // namespace capabilities2_events From e884f2d3560c6e4d0949029a7e17090bd0177e14 Mon Sep 17 00:00:00 2001 From: mik-p Date: Mon, 9 Feb 2026 04:24:34 +0000 Subject: [PATCH 21/37] remove wrong gen uuid, use event gen uuid helper --- .../capabilities2_server/capabilities_api.hpp | 23 +++---------------- 1 file changed, 3 insertions(+), 20 deletions(-) diff --git a/capabilities2_server/include/capabilities2_server/capabilities_api.hpp b/capabilities2_server/include/capabilities2_server/capabilities_api.hpp index 74e4815..ccb913c 100644 --- a/capabilities2_server/include/capabilities2_server/capabilities_api.hpp +++ b/capabilities2_server/include/capabilities2_server/capabilities_api.hpp @@ -7,7 +7,6 @@ #include #include -// #include #include #include @@ -17,6 +16,7 @@ #include #include +#include #include #include @@ -639,7 +639,7 @@ class CapabilitiesAPI const std::string establish_bond(rclcpp::Node::SharedPtr node) { // create a new unique bond id - std::string bond_id = CapabilitiesAPI::gen_bond_id(); + std::string bond_id = capabilities2_events::UUIDGenerator::gen_uuid_str(); // establish a bond and start liveness functions (on_broken, on_formed) // bind the bond id to the callback functions @@ -671,23 +671,6 @@ class CapabilitiesAPI } } -public: - /** - * @brief generate a unique bond id - * - * @return const std::string - */ - // DEPRECATED: use rclcpp UUID generator instead of uuid library for better compatibility and to remove dependency - // static const std::string gen_bond_id() - // { - // // create a new uuid for bond id - // uuid_t uuid; - // uuid_generate_random(uuid); - // char uuid_str[40]; - // uuid_unparse(uuid, uuid_str); - // return std::string(uuid_str); - // } - private: // bind a dependency to an internal bond // this is a bond without a live external connection @@ -698,7 +681,7 @@ class CapabilitiesAPI { // create a new unique bond id // std::string bond_id = CapabilitiesAPI::gen_bond_id(); // DEPRECATED - std::string bond_id = rclcpp::create_uuid_string(); + std::string bond_id = capabilities2_events::UUIDGenerator::gen_uuid_str(); // add the bond id to the bond cache bond_cache_.add_bond(capability, bond_id); From 80dec2bad36f8de292296c8c7d1d5929e5728b88 Mon Sep 17 00:00:00 2001 From: mik-p Date: Mon, 9 Feb 2026 04:30:39 +0000 Subject: [PATCH 22/37] testing runner classes, integrate event node semantics, WIP WIP --- capabilities2_runner/CMakeLists.txt | 2 +- .../capabilities2_runner/action_runner.hpp | 113 +++---- .../capabilities2_runner/encap_runner.hpp | 13 +- .../capabilities2_runner/launch_runner.hpp | 111 +------ .../multiplex_base_runner.hpp | 81 +++-- .../notrigger_action_runner.hpp | 41 --- .../capabilities2_runner/notrigger_runner.hpp | 28 ++ .../capabilities2_runner/runner_base.hpp | 310 ++---------------- .../capabilities2_runner/service_runner.hpp | 139 ++++---- .../get_capability_specs_runner.hpp} | 21 +- .../input_multiplex_all_runner.hpp | 2 +- .../input_multiplex_any_runner.hpp | 2 +- .../threadtrigger_runner.hpp | 163 +++++++++ .../capabilities2_runner/topic_runner.hpp | 116 +++---- capabilities2_runner/package.xml | 10 +- capabilities2_runner/plugins.xml | 4 +- .../src/capability_get_runner.cpp | 6 - capabilities2_runner/src/dummy_runner.cpp | 25 +- .../src/get_capability_specs_runner.cpp | 6 + capabilities2_runner/src/launch_runner.cpp | 2 +- 20 files changed, 474 insertions(+), 721 deletions(-) delete mode 100644 capabilities2_runner/include/capabilities2_runner/notrigger_action_runner.hpp create mode 100644 capabilities2_runner/include/capabilities2_runner/notrigger_runner.hpp rename capabilities2_runner/include/capabilities2_runner/{get_runner.hpp => system/get_capability_specs_runner.hpp} (88%) rename capabilities2_runner/include/capabilities2_runner/{ => system}/input_multiplex_all_runner.hpp (94%) rename capabilities2_runner/include/capabilities2_runner/{ => system}/input_multiplex_any_runner.hpp (95%) create mode 100644 capabilities2_runner/include/capabilities2_runner/threadtrigger_runner.hpp delete mode 100644 capabilities2_runner/src/capability_get_runner.cpp create mode 100644 capabilities2_runner/src/get_capability_specs_runner.cpp diff --git a/capabilities2_runner/CMakeLists.txt b/capabilities2_runner/CMakeLists.txt index 7f356a7..a7d4d80 100644 --- a/capabilities2_runner/CMakeLists.txt +++ b/capabilities2_runner/CMakeLists.txt @@ -24,9 +24,9 @@ include_directories( ) add_library(${PROJECT_NAME} SHARED - src/capability_get_runner.cpp src/dummy_runner.cpp src/launch_runner.cpp + src/get_capability_specs_runner.cpp src/multiplex_all_runner.cpp src/multiplex_any_runner.cpp ) diff --git a/capabilities2_runner/include/capabilities2_runner/action_runner.hpp b/capabilities2_runner/include/capabilities2_runner/action_runner.hpp index 7c768df..42d4fd8 100644 --- a/capabilities2_runner/include/capabilities2_runner/action_runner.hpp +++ b/capabilities2_runner/include/capabilities2_runner/action_runner.hpp @@ -1,7 +1,6 @@ #pragma once #include -#include #include #include #include @@ -11,7 +10,7 @@ #include #include -#include +#include namespace capabilities2_runner { @@ -22,13 +21,13 @@ namespace capabilities2_runner * Create an action client to run an action based capability */ template -class ActionRunner : public RunnerBase +class ActionRunner : public ThreadTriggerRunner { public: /** * @brief Constructor which needs to be empty due to plugin semantics */ - ActionRunner() : RunnerBase() + ActionRunner() : ThreadTriggerRunner() { } @@ -48,22 +47,22 @@ class ActionRunner : public RunnerBase action_client_ = rclcpp_action::create_client(node_, action_name); // wait for action server - info_("waiting for action: " + action_name); + RCLCPP_INFO(node_->get_logger(), "waiting for action: " + action_name); if (!action_client_->wait_for_action_server(std::chrono::seconds(1000))) { - error_("failed to connect to action: " + action_name); + RCLCPP_ERROR(node_->get_logger(), "failed to connect to action: " + action_name); throw runner_exception("failed to connect to action server"); } - info_("connected with action: " + action_name); + RCLCPP_INFO(node_->get_logger(), "connected with action: " + action_name); } /** * @brief stop function to cease functionality and shutdown * */ - virtual void stop() override + virtual void stop(const std::string& bond_id) override { // if the node pointer is empty then throw an error // this means that the runner was not started and is being used out of order @@ -87,17 +86,11 @@ class ActionRunner : public RunnerBase if (response->return_code != action_msgs::srv::CancelGoal_Response::ERROR_NONE) { // throw runner_exception("failed to cancel runner"); - error_("Runner cancellation failed."); + RCLCPP_ERROR(node_->get_logger(), "Runner cancellation failed."); } - // Trigger on_stopped event if defined - if (events[runner_id].on_stopped.interface != "") - { - event_(EventType::STOPPED, -1, events[runner_id].on_stopped.interface, - events[runner_id].on_stopped.provider); - triggerFunction_(events[runner_id].on_stopped.interface, - update_on_stopped(events[runner_id].on_stopped.parameters)); - } + // emit stopped event + emit_stopped(bond_id, update_on_stopped(events[trigger_id].on_stopped.parameters)); }); // wait for action to be stopped. hold the thread for 2 seconds to help keep callbacks in scope @@ -112,21 +105,15 @@ class ActionRunner : public RunnerBase } catch (const rclcpp_action::exceptions::UnknownGoalHandleError& e) { - error_("failed to cancel goal: " + std::string(e.what())); + RCLCPP_ERROR(node_->get_logger(), "failed to cancel goal: " + std::string(e.what())); throw runner_exception(e.what()); } } - info_("removing event options"); - - // remove all event options for this runner instance - const auto n = events.size(); - events.clear(); - info_("removed event options for " + std::to_string(n) + " runner ids"); - - info_("runner cleaned. stopping.."); + RCLCPP_INFO(node_->get_logger(), "runner cleaned. stopping.."); } +protected: /** * @brief Trigger process to be executed. * @@ -134,37 +121,36 @@ class ActionRunner : public RunnerBase * * @param parameters pointer to tinyxml2::XMLElement that contains parameters */ - virtual void execution(int id) override + virtual void execution(const std::string& parameters, const std::string& thread_id) override { + // split thread_id to get bond_id and trigger_id (format: "bond_id/trigger_id") + std::string bond_id = ThreadTriggerRunner::bond_from_thread_id(thread_id); + std::string trigger_id = ThreadTriggerRunner::trigger_from_thread_id(thread_id); + // if parameters are not provided then cannot proceed - if (!parameters_[id]) + if (!parameters_[trigger_id]) throw runner_exception("cannot trigger action without parameters"); // generate a goal from parameters if provided - goal_msg_ = generate_goal(parameters_[id], id); - - info_("goal generated for event ", id); + goal_msg_ = generate_goal(parameters_[trigger_id], trigger_id); + RCLCPP_INFO(node_->get_logger(), "goal generated for event " + trigger_id); std::unique_lock lock(mutex_); completed_ = false; // trigger the action client with goal send_goal_options_.goal_response_callback = - [this, id](const typename rclcpp_action::ClientGoalHandle::SharedPtr& goal_handle) { + [this, trigger_id](const typename rclcpp_action::ClientGoalHandle::SharedPtr& goal_handle) { if (goal_handle) { - info_("goal accepted. Waiting for result", id); - - // trigger the events related to on_started state - if (events[id].on_started.interface != "") - { - event_(EventType::STARTED, id, events[id].on_started.interface, events[id].on_started.provider); - triggerFunction_(events[id].on_started.interface, update_on_started(events[id].on_started.parameters)); - } + RCLCPP_INFO(node_->get_logger(), "goal accepted. Waiting for result " + trigger_id); + + // emit started event + emit_started(bond_id, update_on_started(events[trigger_id].on_started.parameters)); } else { - error_("goal rejected", id); + RCLCPP_ERROR(node_->get_logger(), "goal rejected for event " + trigger_id); } // store goal handle to be used with stop funtion @@ -172,40 +158,32 @@ class ActionRunner : public RunnerBase }; send_goal_options_.feedback_callback = - [this, id](typename rclcpp_action::ClientGoalHandle::SharedPtr goal_handle, - const typename ActionT::Feedback::ConstSharedPtr feedback_msg) { + [this, trigger_id](typename rclcpp_action::ClientGoalHandle::SharedPtr goal_handle, + const typename ActionT::Feedback::ConstSharedPtr feedback_msg) { std::string feedback = generate_feedback(feedback_msg); if (feedback != "") { - info_("received feedback: " + feedback, id); + RCLCPP_INFO(node_->get_logger(), "received feedback: " + feedback + " for event " + trigger_id); } }; send_goal_options_.result_callback = - [this, id](const typename rclcpp_action::ClientGoalHandle::WrappedResult& wrapped_result) { - info_("received result", id); + [this, trigger_id](const typename rclcpp_action::ClientGoalHandle::WrappedResult& wrapped_result) { + RCLCPP_INFO(node_->get_logger(), "received result for event " + trigger_id); if (wrapped_result.code == rclcpp_action::ResultCode::SUCCEEDED) { - info_("action succeeded.", id); - - // trigger the events related to on_success state - if (events[id].on_success.interface != "") - { - event_(EventType::SUCCEEDED, id, events[id].on_success.interface, events[id].on_success.provider); - triggerFunction_(events[id].on_success.interface, update_on_success(events[id].on_success.parameters)); - } + RCLCPP_INFO(node_->get_logger(), "action succeeded for event " + trigger_id); + + // emit success event + emit_succeeded(bond_id, update_on_success(events[trigger_id].on_success.parameters)); } else { - error_("action failed", id); - - // trigger the events related to on_failure state - if (events[id].on_failure.interface != "") - { - event_(EventType::FAILED, id, events[id].on_failure.interface, events[id].on_failure.provider); - triggerFunction_(events[id].on_failure.interface, update_on_failure(events[id].on_failure.parameters)); - } + RCLCPP_ERROR(node_->get_logger(), "action failed for event " + trigger_id); + + // emit failed event + emit_failed(bond_id, update_on_failure(events[trigger_id].on_failure.parameters)); } result_ = wrapped_result.result; @@ -214,14 +192,13 @@ class ActionRunner : public RunnerBase }; goal_handle_future_ = action_client_->async_send_goal(goal_msg_, send_goal_options_); - info_("goal sent. Waiting for acceptance.", id); + RCLCPP_INFO(node_->get_logger(), "goal sent. Waiting for acceptance for event " + trigger_id); // Conditional wait cv_.wait(lock, [this] { return completed_; }); - info_("action complete. Thread closing.", id); + RCLCPP_INFO(node_->get_logger(), "action complete. Thread closing for event " + trigger_id); } -protected: /** * @brief Generate a goal from parameters * @@ -233,7 +210,7 @@ class ActionRunner : public RunnerBase * @param parameters * @return ActionT::Goal the generated goal */ - virtual typename ActionT::Goal generate_goal(tinyxml2::XMLElement* parameters, int id) = 0; + virtual typename ActionT::Goal generate_goal(tinyxml2::XMLElement* parameters, const std::string& trigger_id) = 0; /** * @brief Generate a std::string from feedback message @@ -246,8 +223,10 @@ class ActionRunner : public RunnerBase * @param parameters * @return ActionT::Feedback the received feedback */ - virtual std::string generate_feedback(const typename ActionT::Feedback::ConstSharedPtr msg) = 0; + virtual std::string generate_feedback(const typename ActionT::Feedback::ConstSharedPtr msg, + const std::string& trigger_id) = 0; +protected: /**< action client */ typename rclcpp_action::Client::SharedPtr action_client_; diff --git a/capabilities2_runner/include/capabilities2_runner/encap_runner.hpp b/capabilities2_runner/include/capabilities2_runner/encap_runner.hpp index 931454e..49d3738 100644 --- a/capabilities2_runner/include/capabilities2_runner/encap_runner.hpp +++ b/capabilities2_runner/include/capabilities2_runner/encap_runner.hpp @@ -57,14 +57,17 @@ class EnCapRunner : public NoTriggerActionRunner * call the parent stop and stop the encapsulated action * */ - virtual void stop() override + virtual void stop(const std::string& bond_id) override { // stop the encapsulating action server encap_action_->cancel_all_goals(); encap_action_.reset(); // stop the base class - ActionRunner::stop(); + ActionRunner::stop(bond_id); + + // emit stopped event + emit_stopped(bond_id, ""); } // encapsulated action server related functions @@ -93,12 +96,6 @@ class EnCapRunner : public NoTriggerActionRunner virtual void handle_accepted( const std::shared_ptr> goal_handle); - /** - * @brief execute the encapsulated action request - * - */ - virtual void execute(); - private: /** encap action server */ std::shared_ptr> encap_action_; diff --git a/capabilities2_runner/include/capabilities2_runner/launch_runner.hpp b/capabilities2_runner/include/capabilities2_runner/launch_runner.hpp index fe2dc83..ccab5c9 100644 --- a/capabilities2_runner/include/capabilities2_runner/launch_runner.hpp +++ b/capabilities2_runner/include/capabilities2_runner/launch_runner.hpp @@ -1,7 +1,6 @@ #pragma once -#include -#include +#include namespace capabilities2_runner { @@ -10,16 +9,15 @@ namespace capabilities2_runner * @brief launch runner base class * * Create a launch file runner to run a launch file based capability + * uses process execution to run the launch file and kill the process on stop */ -class LaunchRunner : public RunnerBase +class LaunchRunner : public NoTriggerRunner { public: - using Launch = capabilities2_msgs::srv::Launch; - /** * @brief Constructor which needs to be empty due to plugin semantics */ - LaunchRunner() : RunnerBase() + LaunchRunner() : NoTriggerRunner() { } @@ -28,74 +26,27 @@ class LaunchRunner : public RunnerBase * * @param node shared pointer to the capabilities node. Allows to use ros node related functionalities * @param run_config runner configuration loaded from the yaml file + * @param bond_id bond identifier for the runner */ - virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config) override + virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config, const std::string& bond_id) override { init_base(node, run_config); package_name = run_config_.runner.substr(0, run_config_.runner.find("/")); launch_name = run_config_.runner.substr(run_config_.runner.find("/") + 1); - // create an service client - start_service_client_ = node_->create_client("/capabilities/launch/start"); - - RCLCPP_INFO(node_->get_logger(), "%s waiting for service: /capabilities/launch/start", - run_config_.interface.c_str()); - - if (!start_service_client_->wait_for_service(std::chrono::seconds(3))) - { - RCLCPP_ERROR(node_->get_logger(), "%s failed to connect to service: /capabilities/launch/start", - run_config_.interface.c_str()); - throw runner_exception("Failed to connect to server: /capabilities/launch/start"); - } - - RCLCPP_INFO(node_->get_logger(), "%s connected to service: /capabilities/launch/start", - run_config_.interface.c_str()); - - // create an service client - stop_service_client_ = node_->create_client("/capabilities/launch/stop"); - - // wait for action server - RCLCPP_INFO(node_->get_logger(), "%s waiting for service: /capabilities/launch/stop", - run_config_.interface.c_str()); - - if (!stop_service_client_->wait_for_service(std::chrono::seconds(3))) - { - RCLCPP_ERROR(node_->get_logger(), "%s failed to connect to service: /capabilities/launch/stop", - run_config_.interface.c_str()); - throw runner_exception("Failed to connect to server: /capabilities/launch/stop"); - } - - RCLCPP_INFO(node_->get_logger(), "%s connected to service: /capabilities/launch/stop", - run_config_.interface.c_str()); - - // generate a reequest from launch_name and package_name - auto request_msg = std::make_shared(); - - request_msg->package_name = package_name; - request_msg->launch_file_name = launch_name; - - RCLCPP_INFO(node_->get_logger(), "Requesting to launch %s from %s", launch_name.c_str(), package_name.c_str()); - - auto result_future = start_service_client_->async_send_request( - request_msg, [this](typename rclcpp::Client::SharedFuture future) { - if (!future.valid()) - { - RCLCPP_ERROR(node_->get_logger(), "Request to launch %s from %s failed", launch_name.c_str(), - package_name.c_str()); - return; - } + // start launch process + throw runner_exception("launch runner not implemented yet"); - RCLCPP_INFO(node_->get_logger(), "Request to launch %s from %s succeeded", launch_name.c_str(), - package_name.c_str()); - }); + // emit started event + emit_started(bond_id, "launch started"); } /** * @brief stop function to cease functionality and shutdown * */ - virtual void stop() override + virtual void stop(const std::string& bond_id) override { // if the node pointer is empty then throw an error // this means that the runner was not started and is being used out of order @@ -103,48 +54,16 @@ class LaunchRunner : public RunnerBase if (!node_) throw runner_exception("cannot stop runner that was not started"); - // generate a reequest from launch_name and package_name - auto request_msg = std::make_shared(); + // stop the launch process + throw runner_exception("launch runner not implemented yet"); - request_msg->package_name = package_name; - request_msg->launch_file_name = launch_name; - - RCLCPP_INFO(node_->get_logger(), "Requesting to stop %s from %s", launch_name.c_str(), package_name.c_str()); - - auto result_future = stop_service_client_->async_send_request( - request_msg, [this](typename rclcpp::Client::SharedFuture future) { - if (!future.valid()) - { - RCLCPP_ERROR(node_->get_logger(), "Request to stop %s from %s failed ", launch_name.c_str(), - package_name.c_str()); - return; - } - - RCLCPP_INFO(node_->get_logger(), "Request to launch %s from %s succeeded ", launch_name.c_str(), - package_name.c_str()); - }); - - info_("stopping runner"); - } - - // throw on trigger function - void trigger(const std::string& parameters) override - { - throw runner_exception("No Trigger as this is launch runner"); + // emit stopped event + emit_stopped(bond_id, "launch stopped"); } protected: - // throw on triggerExecution function - void execution(int id) override - { - throw runner_exception("no triggerExecution() this is a no-trigger action runner"); - } - std::string launch_name; std::string package_name; - - rclcpp::Client::SharedPtr start_service_client_; - rclcpp::Client::SharedPtr stop_service_client_; }; } // namespace capabilities2_runner diff --git a/capabilities2_runner/include/capabilities2_runner/multiplex_base_runner.hpp b/capabilities2_runner/include/capabilities2_runner/multiplex_base_runner.hpp index 7eb0d34..86f85e7 100644 --- a/capabilities2_runner/include/capabilities2_runner/multiplex_base_runner.hpp +++ b/capabilities2_runner/include/capabilities2_runner/multiplex_base_runner.hpp @@ -1,9 +1,6 @@ #pragma once -#include -#include -#include -#include +#include namespace capabilities2_runner { @@ -14,13 +11,13 @@ namespace capabilities2_runner * Base class for inter-runner connections that require multiplexing of inputs * */ -class MultiplexBaseRunner : public RunnerBase +class MultiplexBaseRunner : public ThreadTriggerRunner { public: /** * @brief Constructor which needs to be empty due to plugin semantics */ - MultiplexBaseRunner() : RunnerBase() + MultiplexBaseRunner() : ThreadTriggerRunner() { } @@ -30,61 +27,63 @@ class MultiplexBaseRunner : public RunnerBase * @param node shared pointer to the capabilities node. Allows to use ros node related functionalities * @param run_config runner configuration loaded from the yaml file */ - virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config) override + virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config, const std::string& bond_id) override { init_base(node, run_config); + + // emit started event + emit_started(bond_id, ""); + } + + /** + * @brief stop function to cease functionality and shutdown + * + */ + virtual void stop(const std::string& bond_id) override + { + // if the node pointer is empty then throw an error + // this means that the runner was not started and is being used out of order + + if (!node_) + throw runner_exception("cannot stop runner that was not started"); + + // emit stopped event + emit_stopped(bond_id, update_on_stopped(events[runner_id].on_stopped.parameters)); + + RCLCPP_INFO(node_->get_logger(), "stopping runner"); } +protected: /** * @brief Trigger process to be executed. * * @param id unique identifier for the execution */ - virtual void execution(int id) + virtual void execution(const std::string& parameters, const std::string& thread_id) override { - info_("execution started for id: " + std::to_string(id)); + // extract trigger_id from thread_id (format: "bond_id/trigger_id") + size_t slash_pos = thread_id.find('/'); + std::string trigger_id = (slash_pos != std::string::npos) ? thread_id.substr(slash_pos + 1) : ""; + + RCLCPP_INFO(node_->get_logger(), "execution started for trigger_id: %s", trigger_id.c_str()); // check if the id is already completed - if (completed_executions.find(id) != completed_executions.end() && completed_executions[id]) + if (completed_executions.find(trigger_id) != completed_executions.end() && completed_executions[trigger_id]) { - info_("execution already completed for id: " + std::to_string(id)); + RCLCPP_INFO(node_->get_logger(), "execution already completed for trigger_id: %s", trigger_id.c_str()); return; } else { - // trigger the events related to on_success state - if (events[id].on_success.interface != "") - { - event_(EventType::SUCCEEDED, id, events[id].on_success.interface, events[id].on_success.provider); - triggerFunction_(events[id].on_success.interface, update_on_success(events[id].on_success.parameters)); - } - // trigger the events related to on_failure state - else if (events[id].on_failure.interface != "") - { - event_(EventType::FAILED, id, events[id].on_failure.interface, events[id].on_failure.provider); - triggerFunction_(events[id].on_failure.interface, update_on_failure(events[id].on_failure.parameters)); - } + // emit events based on the execution result + // TODO: determine execution result and emit appropriate events, currently emits success for demonstration + // used to emit success and failure? } // track the execution as completed - completed_executions[id] = true; - - info_("multiplexing complete. Thread closing.", id); - } - - /** - * @brief stop function to cease functionality and shutdown - * - */ - virtual void stop() override - { - // if the node pointer is empty then throw an error - // this means that the runner was not started and is being used out of order - - if (!node_) - throw runner_exception("cannot stop runner that was not started"); + completed_executions[trigger_id] = true; - info_("stopping runner"); + RCLCPP_INFO(node_->get_logger(), "multiplexing complete. Thread closing for trigger_id: %s", trigger_id.c_str()); } protected: @@ -95,7 +94,7 @@ class MultiplexBaseRunner : public RunnerBase std::map expected_input_count; // completed executions - std::map completed_executions; + std::map completed_executions; }; } // namespace capabilities2_runner diff --git a/capabilities2_runner/include/capabilities2_runner/notrigger_action_runner.hpp b/capabilities2_runner/include/capabilities2_runner/notrigger_action_runner.hpp deleted file mode 100644 index 1287105..0000000 --- a/capabilities2_runner/include/capabilities2_runner/notrigger_action_runner.hpp +++ /dev/null @@ -1,41 +0,0 @@ -#pragma once - -#include - -namespace capabilities2_runner -{ - -/** - * @brief no-trigger action runner - * - * provides a no trigger runner implementation for the action runner - * use for child action classes that do not require a trigger - * - * @tparam ActionT - */ -template -class NoTriggerActionRunner : public ActionRunner -{ -public: - // throw on trigger function - void trigger(tinyxml2::XMLElement* parameters) override - { - throw runner_exception("cannot trigger this is a no-trigger action runner"); - } - -protected: - - // throw on triggerExecution function - void execution() override - { - throw runner_exception("no triggerExecution() this is a no-trigger action runner"); - } - - // throw on xml conversion functions - typename ActionT::Goal generate_goal(tinyxml2::XMLElement*) override - { - throw runner_exception("cannot generate goal this is a no-trigger action runner"); - } -}; - -} // namespace capabilities2_runner diff --git a/capabilities2_runner/include/capabilities2_runner/notrigger_runner.hpp b/capabilities2_runner/include/capabilities2_runner/notrigger_runner.hpp new file mode 100644 index 0000000..6c0bea0 --- /dev/null +++ b/capabilities2_runner/include/capabilities2_runner/notrigger_runner.hpp @@ -0,0 +1,28 @@ +#pragma once + +#include + +namespace capabilities2_runner +{ + +/** + * @brief no-trigger runner + * + * provides a no trigger runner implementation for the runner + * use for child runner classes that do not require a trigger + * + */ +class NoTriggerRunner : public RunnerBase +{ +public: + // throw on trigger function + void trigger(const std::string& parameters, const std::string& bond_id) override + { + // emit failed event + emit_failed(bond_id, "cannot trigger this is a no-trigger runner"); + + throw runner_exception("cannot trigger this is a no-trigger runner"); + } +}; + +} // namespace capabilities2_runner diff --git a/capabilities2_runner/include/capabilities2_runner/runner_base.hpp b/capabilities2_runner/include/capabilities2_runner/runner_base.hpp index 1ab4c38..76b3c0d 100644 --- a/capabilities2_runner/include/capabilities2_runner/runner_base.hpp +++ b/capabilities2_runner/include/capabilities2_runner/runner_base.hpp @@ -3,13 +3,14 @@ #include #include #include -#include -#include -#include + #include #include +#include +#include + namespace capabilities2_runner { /** @@ -72,67 +73,11 @@ struct runner_opts class RunnerBase : public capabilities2_events::EventNode { public: - // static xml conversion - - /** - * @brief convert an XMLElement to std::string - * - * @param element XMLElement element to be converted - * @param parameters parameter to hold std::string - * - * @return `true` if element is not nullptr and conversion successful, `false` if element is nullptr - */ - static const std::string convert_to_string(tinyxml2::XMLElement* element) + RunnerBase() : node_(nullptr), run_config_() { - if (element) - { - tinyxml2::XMLPrinter printer; - - element->Accept(&printer); - std::string parameters = printer.CStr(); - return parameters; - } - else - { - std::string parameters = ""; - return parameters; - } } - /** - * @brief convert an XMLElement to std::string - * - * @param element XMLElement element to be converted - * @param parameters parameter to hold std::string - * - * @return `true` if element is not nullptr and conversion successful, `false` if element is nullptr - */ - static tinyxml2::XMLElement* convert_to_xml(const std::string& parameters) - { - tinyxml2::XMLDocument doc; - - if (parameters != "") - { - doc.Parse(parameters.c_str()); - tinyxml2::XMLElement* element = doc.FirstChildElement(); - return element; - } - else - { - return nullptr; - } - } - -public: - RunnerBase() : run_config_(), execution_should_stop_(false) - { - } - - ~RunnerBase() - { - // clean up threads on destruction - stop_execution(std::chrono::milliseconds(500)); - } + ~RunnerBase() = default; /** runner plugin api */ @@ -147,7 +92,7 @@ class RunnerBase : public capabilities2_events::EventNode * NOTE: must call init_base in derived class implementation * NOTE: should call start event */ - virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config) = 0; + virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config, const std::string& bond_id) = 0; /** * @brief stop the runner @@ -155,7 +100,7 @@ class RunnerBase : public capabilities2_events::EventNode * NOTE: should clean up threads * NOTE: should call stop event */ - virtual void stop() = 0; + virtual void stop(const std::string& bond_id) = 0; /** * FIXME: implement new event subsystem @@ -169,53 +114,27 @@ class RunnerBase : public capabilities2_events::EventNode * @param bond_id unique identifier for the group of connections associated with this runner trigger event * */ - virtual void trigger(const std::string& parameters, const std::string& bond_id) - { - // TODO: verify parameter formatting (safe xml string or other format) - // TODO: minimum parameter set? - - // DEPRECATED: extract trigger id from parameters - // // extract the unique id for the runner and use that as the thread id - // tinyxml2::XMLElement* element = nullptr; - // element = convert_to_xml(parameters); - // if (!element) - // { - // // when this is empty it means that the trigger activation was performed - // // by a registered user - // // this is valid but we need to get a unique trigger id for the runner - // // to proceed safely - // RCLCPP_WARN(node_->get_logger(), "no trigger parameters provided"); - // } - - // std::string trigger_id = ""; - // element->QueryStringAttribute("id", &trigger_id); - - // parameters_[trigger_id] = element; - - // create a thread id - std::string trigger_id = rclcpp::create_uuid_string(); - // namespace the thread id with bond id for later - // could list all threads related to a bond if needed - std::string thread_id = bond_id + "/" + trigger_id; - - // start execution thread - { - std::scoped_lock lock(mutex_); - // TODO: consider emitting on start event here - // emit_event(bond_id, capabilities2_msgs::msg::CapabilityEventCode::ON_STARTED, updated_on_started(parameters)); - execution_thread_pool_[thread_id] = std::thread(&RunnerBase::execution, this, parameters, bond_id); - } + // TODO: verify parameter formatting (safe xml string or other format) + // TODO: minimum parameter set? - // emit trigger event - emit_event(bond_id, capabilities2_msgs::msg::CapabilityEventCode::TRIGGERED, ("")); - - // BUG: thread management? + // DEPRECATED: extract trigger id from parameters + // // extract the unique id for the runner and use that as the thread id + // tinyxml2::XMLElement* element = nullptr; + // element = convert_to_xml(parameters); + // if (!element) + // { + // // when this is empty it means that the trigger activation was performed + // // by a registered user + // // this is valid but we need to get a unique trigger id for the runner + // // to proceed safely + // RCLCPP_WARN(node_->get_logger(), "no trigger parameters provided"); + // } - // TODO: consider emitting on stop event here - // emit_event(bond_id, capabilities2_msgs::msg::CapabilityEventCode::ON_STOPPED, updated_on_stopped(parameters)); + // std::string trigger_id = ""; + // element->QueryStringAttribute("id", &trigger_id); - RCLCPP_DEBUG(node_->get_logger(), "started execution thread for runner id: %s", trigger_id.c_str()); - } + // parameters_[trigger_id] = element; + virtual void trigger(const std::string& parameters, const std::string& bond_id) = 0; /** * @brief Initializer function for initializing the base runner in place of constructor due to plugin semantics @@ -234,11 +153,6 @@ class RunnerBase : public capabilities2_events::EventNode // store node pointer and opts node_ = node; run_config_ = run_config; - - // FIXME: should be in derived class? - // current_inputs_ = 0; - // DEPRECATED: event node keeps a unique id - // runner_id = 0; } /** @@ -254,25 +168,6 @@ class RunnerBase : public capabilities2_events::EventNode } } - // FIXME: implement new event subsystem - /** - * @brief attach events to the runner - * - * @param event_option event_options related for the action - * @param triggerFunction external function that triggers capability runners - * - * @return number of attached events - */ - // MOVED TO: capabilities2_events::EventNode - // virtual void attach_events(capabilities2::event_opts& event_option, - // std::function triggerFunction) - // { - // // info_("accepted event options with ID : " + std::to_string(event_option.event_id)); - - // triggerFunction_ = triggerFunction; - - // events[event_option.event_id] = event_option; - // } /** * @brief make EventNode::add_connection public method on runner api * @@ -333,59 +228,7 @@ class RunnerBase : public capabilities2_events::EventNode return run_config_.pid; } - // FIXME: not used? - /** - * @brief Get the execution status of runner. - * - * @return `true` if execution is complete, `false` otherwise. - */ - // const bool get_completion_status() const - // { - // return execution_complete_; - // } - protected: - /** - * @brief Trigger process to be executed. - * - * This method utilizes parameters set via the trigger() function - * - * @param parameters parameters for the execution - * - * NOTE: should call success and failure events appropriately - */ - virtual void execution(const std::string& parameters, const std::string& bond_id) = 0; - - /** */ - void stop_execution(std::chrono::milliseconds timeout = std::chrono::milliseconds(500)) - { - // signal listening threads to stop - execution_should_stop_ = true; - - // try join in a non-blocking way and log if threads are not cleaned up properly - { - std::scoped_lock lock(mutex_); - for (auto& [thread_id, exec_thread] : execution_thread_pool_) - { - if (exec_thread.joinable()) - { - if (exec_thread.try_join_for(timeout)) - { - RCLCPP_DEBUG(node_->get_logger(), "execution %s joined successfully", thread_id.c_str()); - } - else - { - RCLCPP_ERROR(node_->get_logger(), "execution %s did not stop in time, detaching", thread_id.c_str()); - exec_thread.detach(); // don't block, but log the issue - } - } - } - - // clear the thread pool - execution_thread_pool_.clear(); - } - } - // FIXME: implement new event subsystem // STATE CHANGE PARAMETER HELPERS @@ -586,7 +429,6 @@ class RunnerBase : public capabilities2_events::EventNode return get_first_resource_name("action"); } -protected: // FIXME: implement new event subsystem // STATE CHANGE HELPERS @@ -598,9 +440,7 @@ class RunnerBase : public capabilities2_events::EventNode */ void emit_started(const std::string& bond_id, const std::string& parameters = "") { - capabilities2_msgs::msg::CapabilityEventCode event_type; - event_type.code = capabilities2_msgs::msg::CapabilityEventCode::STARTED; - emit_event(bond_id, event_type, parameters); + emit_event(bond_id, capabilities2_msgs::msg::CapabilityEventCode::STARTED, parameters); } /** @@ -611,9 +451,7 @@ class RunnerBase : public capabilities2_events::EventNode */ void emit_stopped(const std::string& bond_id, const std::string& parameters = "") { - capabilities2_msgs::msg::CapabilityEventCode event_type; - event_type.code = capabilities2_msgs::msg::CapabilityEventCode::STOPPED; - emit_event(bond_id, event_type, parameters); + emit_event(bond_id, capabilities2_msgs::msg::CapabilityEventCode::STOPPED, parameters); } /** @@ -624,9 +462,7 @@ class RunnerBase : public capabilities2_events::EventNode */ void emit_succeeded(const std::string& bond_id, const std::string& parameters = "") { - capabilities2_msgs::msg::CapabilityEventCode event_type; - event_type.code = capabilities2_msgs::msg::CapabilityEventCode::SUCCEEDED; - emit_event(bond_id, event_type, parameters); + emit_event(bond_id, capabilities2_msgs::msg::CapabilityEventCode::SUCCEEDED, parameters); } /** @@ -637,11 +473,10 @@ class RunnerBase : public capabilities2_events::EventNode */ void emit_failed(const std::string& bond_id, const std::string& parameters = "") { - capabilities2_msgs::msg::CapabilityEventCode event_type; - event_type.code = capabilities2_msgs::msg::CapabilityEventCode::FAILED; - emit_event(bond_id, event_type, parameters); + emit_event(bond_id, capabilities2_msgs::msg::CapabilityEventCode::FAILED, parameters); } +protected: /** * @brief shared pointer to the capabilities node * Allows to use ros node related functionalities @@ -652,89 +487,6 @@ class RunnerBase : public capabilities2_events::EventNode * @brief run_config_ runner configuration */ runner_opts run_config_; - - /** - * @brief dictionary of events - */ - // DEPRECATED: moved to capabilities2_events::EventNode - // std::map events; - - /** - * @brief unique id for the runner - */ - // DEPRECATED: moved eventNode which uniquely identifies itself - // int runner_id; - - // FIXME: should be moved to derived class - /** - * @brief current number of trigger signals received - */ - // int current_inputs_; - - // FIXME: should be moved to derived class - /** - * @brief system runner completion tracking - */ - // bool execution_complete_; - - /** - * @brief pointer to XMLElement which contain parameters - * NOTE: std::string is thread/runner id - */ - // DEPRECATED: moved to execution function parameter since events track elsewhere - // just need to insert parameters into event emission during execution - // std::map parameters_; - - /** - * @brief dictionary of threads that executes the execute function - */ - std::map execution_thread_pool_; - - /** - * @brief mutex for threadpool synchronisation. - */ - std::mutex mutex_; - - /** - * @brief flag to signal execution threads to stop. - */ - std::atomic execution_should_stop_; - - /** - * @brief conditional variable for threadpool synchronisation. - */ - // FIXME: move for derived class use - // std::condition_variable cv_; - - /** - * @brief flag for threadpool synchronisation. - */ - // FIXME: move for derived class use - // bool completed_; - - // FIXME: implement new event subsystem - /** - * @brief external function that triggers capability runners - */ - // DEPRECATED: moved to capabilities2_events::EventNode - // std::function triggerFunction_; - - // TODO: try make static helper class for xml conversion - /** - * @brief XMLElement that is used to convert xml strings to std::string - */ - // tinyxml2::XMLPrinter printer; - - /** - * @brief XMLElement that is used to convert std::string to xml strings - */ - // tinyxml2::XMLDocument doc; - - /** - * @brief client api for event emission - */ - // DEPRECATED: moved to capabilities2_events::EventNode - // std::shared_ptr event_; }; } // namespace capabilities2_runner diff --git a/capabilities2_runner/include/capabilities2_runner/service_runner.hpp b/capabilities2_runner/include/capabilities2_runner/service_runner.hpp index d28f6be..0821595 100644 --- a/capabilities2_runner/include/capabilities2_runner/service_runner.hpp +++ b/capabilities2_runner/include/capabilities2_runner/service_runner.hpp @@ -1,9 +1,8 @@ #pragma once -#include "rclcpp/rclcpp.hpp" +#include -#include -#include +#include namespace capabilities2_runner { @@ -14,13 +13,13 @@ namespace capabilities2_runner * Create an server client to run an service based capability */ template -class ServiceRunner : public RunnerBase +class ServiceRunner : public ThreadTriggerRunner { public: /** * @brief Constructor which needs to be empty due to plugin semantics */ - ServiceRunner() : RunnerBase() + ServiceRunner() : ThreadTriggerRunner() { } @@ -37,120 +36,100 @@ class ServiceRunner : public RunnerBase // initialize the runner base by storing node pointer and run config init_base(node, run_config); - // create an service client + // create a service client service_client_ = node_->create_client(service_name); - // wait for action server - info_("waiting for service: " + service_name); + // wait for service server + RCLCPP_INFO(node_->get_logger(), "waiting for service: " + service_name); if (!service_client_->wait_for_service(std::chrono::seconds(3))) { - error_("failed to connect to service: " + service_name); + RCLCPP_ERROR(node_->get_logger(), "failed to connect to service: " + service_name); throw runner_exception("failed to connect to server"); } - info_("connected with service: " + service_name); + RCLCPP_INFO(node_->get_logger(), "connected with service: " + service_name); } + /** + * @brief stop function to cease functionality and shutdown + * + */ + virtual void stop(const std::string& bond_id) override + { + // if the node pointer is empty then throw an error + // this means that the runner was not started and is being used out of order + + if (!node_) + throw runner_exception("cannot stop runner that was not started"); + + // throw an error if the service client is null + // this can happen if the runner is not able to find the action resource + + if (!service_client_) + throw runner_exception("cannot stop runner action that was not started"); + + // emit stopped event + emit_stopped(bond_id, update_on_stopped(events[runner_id].on_stopped.parameters)); + + RCLCPP_INFO(node_->get_logger(), "runner cleaned. stopping.."); + } + +protected: /** * @brief Trigger process to be executed. * * This method utilizes paramters set via the trigger() function * * @param parameters pointer to tinyxml2::XMLElement that contains parameters + * @param thread_id unique identifier for the execution thread */ - virtual void execution(int id) override + virtual void execution(const std::string& parameters, const std::string& thread_id) override { + // split thread_id to get bond_id and trigger_id (format: "bond_id/trigger_id") + std::string bond_id = ThreadTriggerRunner::bond_from_thread_id(thread_id); + std::string trigger_id = ThreadTriggerRunner::trigger_from_thread_id(thread_id); + // if parameters are not provided then cannot proceed - if (!parameters_[id]) + if (!parameters_[trigger_id]) throw runner_exception("cannot trigger service without parameters"); // generate a goal from parameters if provided - auto request_msg = std::make_shared(generate_request(parameters_[id], id)); + auto request_msg = + std::make_shared(generate_request(parameters_[trigger_id], trigger_id)); - info_("request generated for event :", id); + RCLCPP_INFO(node_->get_logger(), "request generated for event :" + trigger_id); std::unique_lock lock(mutex_); completed_ = false; auto result_future = service_client_->async_send_request( - request_msg, [this, id](typename rclcpp::Client::SharedFuture future) { + request_msg, [this, trigger_id](typename rclcpp::Client::SharedFuture future) { if (!future.valid()) { - error_("get result call failed"); - - // trigger the events related to on_failure state - if (events[id].on_failure.interface != "") - { - event_(EventType::FAILED, id, events[id].on_failure.interface, events[id].on_failure.provider); - triggerFunction_(events[id].on_failure.interface, update_on_failure(events[id].on_failure.parameters)); - } + RCLCPP_ERROR(node_->get_logger(), "get result call failed"); + + // emit failed event + emit_failed(bond_id, update_on_failure(events[trigger_id].on_failure.parameters)); } else { - info_("get result call succeeded", id); + RCLCPP_INFO(node_->get_logger(), "get result call succeeded for event :" + trigger_id); response_ = future.get(); - process_response(response_, id); - - // trigger the events related to on_success state - if (events[id].on_success.interface != "") - { - event_(EventType::SUCCEEDED, id, events[id].on_success.interface, events[id].on_success.provider); - triggerFunction_(events[id].on_success.interface, update_on_success(events[id].on_success.parameters)); - } + process_response(response_, trigger_id); + + // emit success event + emit_succeeded(bond_id, update_on_success(events[trigger_id].on_success.parameters)); } completed_ = true; cv_.notify_all(); }); - // trigger the events related to on_started state - if (events[id].on_started.interface != "") - { - event_(EventType::STARTED, id, events[id].on_started.interface, events[id].on_started.provider); - triggerFunction_(events[id].on_started.interface, update_on_started(events[id].on_started.parameters)); - } - // Conditional wait cv_.wait(lock, [this] { return completed_; }); - info_("Service request complete. Thread closing.", id); - } - - /** - * @brief stop function to cease functionality and shutdown - * - */ - virtual void stop() override - { - // if the node pointer is empty then throw an error - // this means that the runner was not started and is being used out of order - - if (!node_) - throw runner_exception("cannot stop runner that was not started"); - - // throw an error if the service client is null - // this can happen if the runner is not able to find the action resource - - if (!service_client_) - throw runner_exception("cannot stop runner action that was not started"); - - // Trigger on_stopped event if defined - if (events[runner_id].on_stopped.interface != "") - { - event_(EventType::STOPPED, -1, events[runner_id].on_stopped.interface, events[runner_id].on_stopped.provider); - triggerFunction_(events[runner_id].on_stopped.interface, - update_on_stopped(events[runner_id].on_stopped.parameters)); - } - - info_("removing event options"); - - // remove all event options for this runner instance - const auto n = events.size(); - events.clear(); - info_("removed event options for " + std::to_string(n) + " runner ids"); - - info_("runner cleaned. stopping.."); + RCLCPP_INFO(node_->get_logger(), "Service request complete. Thread closing."); } protected: @@ -165,19 +144,19 @@ class ServiceRunner : public RunnerBase * @param parameters * @return ServiceT::Request the generated request */ - virtual typename ServiceT::Request generate_request(tinyxml2::XMLElement* parameters, int id) = 0; + virtual typename ServiceT::Request generate_request(const std::string& parameters, const std::string& trigger_id) = 0; /** * @brief Process the reponse and print data as required * * @param response service reponse - * @param id thread id + * @param trigger_id thread id */ - virtual void process_response(typename ServiceT::Response::SharedPtr response, int id) + virtual void process_response(typename ServiceT::Response::SharedPtr response, const std::string& trigger_id) { } typename rclcpp::Client::SharedPtr service_client_; typename ServiceT::Response::SharedPtr response_; }; -} // namespace capabilities2_runner \ No newline at end of file +} // namespace capabilities2_runner diff --git a/capabilities2_runner/include/capabilities2_runner/get_runner.hpp b/capabilities2_runner/include/capabilities2_runner/system/get_capability_specs_runner.hpp similarity index 88% rename from capabilities2_runner/include/capabilities2_runner/get_runner.hpp rename to capabilities2_runner/include/capabilities2_runner/system/get_capability_specs_runner.hpp index 113c002..12d4d4b 100644 --- a/capabilities2_runner/include/capabilities2_runner/get_runner.hpp +++ b/capabilities2_runner/include/capabilities2_runner/system/get_capability_specs_runner.hpp @@ -1,12 +1,7 @@ #pragma once -#include - -#include -#include -#include - #include + #include namespace capabilities2_runner_capabilities @@ -18,10 +13,10 @@ namespace capabilities2_runner_capabilities * Class to run capabilities2 executor action based capability * */ -class CapabilityGetRunner : public ServiceRunner +class GetCapabilitySpecsRunner : public ServiceRunner { public: - CapabilityGetRunner() : ServiceRunner() + GetCapabilitySpecsRunner() : ServiceRunner() { } @@ -30,8 +25,9 @@ class CapabilityGetRunner : public ServiceRunner' * @return ActionT::Goal the generated goal */ - virtual capabilities2_msgs::srv::GetCapabilitySpecs::Request generate_request(tinyxml2::XMLElement* parameters, - int id) override + virtual capabilities2_msgs::srv::GetCapabilitySpecs::Request generate_request(const std::string& parameters, + const std::string& trigger_id) override { capabilities2_msgs::srv::GetCapabilitySpecs::Request request; return request; } /** - * @brief Update on_success event parameters with new data from an CapabilitySpecs message if avaible. + * @brief Update on_success event parameters with new data from an CapabilitySpecs message if available. * * This function is used to inject new data into the XMLElement containing * parameters related to the on_success trigger event @@ -76,7 +72,6 @@ class CapabilityGetRunner : public ServiceRunner +#include +#include +#include +#include + +#include + +#include + +#include + +namespace capabilities2_runner +{ + +/** + * @brief add threaded trigger execution to the runner + * + */ +class ThreadTriggerRunner : public RunnerBase +{ +public: + /** + * @brief helper function to extract bond_id from thread_id + * + * @param thread_id + * @return const std::string + */ + static const std::string bond_from_thread_id(const std::string& thread_id) + { + // extract bond_id from thread_id (format: "bond_id/trigger_id") + size_t slash_pos = thread_id.find('/'); + std::string bond_id = (slash_pos != std::string::npos) ? thread_id.substr(0, slash_pos) : thread_id; + return bond_id; + } + + /** + * @brief helper function to extract trigger_id from thread_id + * + * @param thread_id + * @return const std::string + */ + static const std::string trigger_from_thread_id(const std::string& thread_id) + { + // extract trigger_id from thread_id (format: "bond_id/trigger_id") + size_t slash_pos = thread_id.find('/'); + std::string trigger_id = (slash_pos != std::string::npos) ? thread_id.substr(slash_pos + 1) : ""; + return trigger_id; + } + +public: + ThreadTriggerRunner() : RunnerBase(), execution_thread_pool_(), mutex_(), execution_should_stop_(false) + { + } + + ~ThreadTriggerRunner() + { + // stop any running threads on destruction + stop_execution(std::chrono::milliseconds(500)); + } + + /** + * @brief Trigger the runner + * + * This method allows insertion of parameters in a runner after it has been initialized. it is an approach + * to parameterise capabilities. Internally starts up RunnerBase::triggerExecution in a thread + * + * @param parameters pointer to tinyxml2::XMLElement that contains parameters + * @param bond_id unique identifier for the group of connections associated with this runner trigger event + * + */ + virtual void trigger(const std::string& parameters, const std::string& bond_id) override + { + // create a thread id + std::string trigger_id = capabilities2_events::UUIDGenerator::gen_uuid_str(); + // namespace the thread id with bond id for later + // could list all threads related to a bond if needed + std::string thread_id = bond_id + "/" + trigger_id; + + // start execution thread + { + std::scoped_lock lock(mutex_); + // TODO: consider emitting on start event here + // emit_event(bond_id, capabilities2_msgs::msg::CapabilityEventCode::ON_STARTED, updated_on_started(parameters)); + execution_thread_pool_[thread_id] = std::thread(&RunnerBase::execution, this, parameters, thread_id); + } + + // emit trigger event + emit_event(bond_id, capabilities2_msgs::msg::CapabilityEventCode::TRIGGERED, ("")); + + // BUG: thread management? + + // TODO: consider emitting on stop event here + // emit_event(bond_id, capabilities2_msgs::msg::CapabilityEventCode::ON_STOPPED, updated_on_stopped(parameters)); + + RCLCPP_DEBUG(node_->get_logger(), "started execution thread for runner id: %s", trigger_id.c_str()); + } + +protected: + // execution function to be implemented by child classes + /** + * @brief Trigger process to be executed. + * + * This method utilizes parameters set via the trigger() function + * + * @param parameters parameters for the execution + * @param thread_id unique identifier for the execution thread, can be used for tracking and cleanup + * + * NOTE: should call success and failure events appropriately + */ + virtual void execution(const std::string& parameters, const std::string& thread_id) = 0; + +private: + /** */ + void stop_execution(std::chrono::milliseconds timeout = std::chrono::milliseconds(500)) + { + // signal listening threads to stop + execution_should_stop_ = true; + + // try join in a non-blocking way and log if threads are not cleaned up properly + { + std::scoped_lock lock(mutex_); + for (auto& [thread_id, exec_thread] : execution_thread_pool_) + { + if (exec_thread.joinable()) + { + if (exec_thread.try_join_for(timeout)) + { + RCLCPP_DEBUG(node_->get_logger(), "execution %s joined successfully", thread_id.c_str()); + } + else + { + RCLCPP_ERROR(node_->get_logger(), "execution %s did not stop in time, detaching", thread_id.c_str()); + exec_thread.detach(); // don't block, but log the issue + } + } + } + + // clear the thread pool + execution_thread_pool_.clear(); + } + } + +protected: + /** + * @brief dictionary of threads that executes the execute function + */ + std::map execution_thread_pool_; + + /** + * @brief mutex for threadpool synchronisation. + */ + std::mutex mutex_; + + /** + * @brief flag to signal execution threads to stop. + */ + std::atomic execution_should_stop_; +}; + +} // namespace capabilities2_runner diff --git a/capabilities2_runner/include/capabilities2_runner/topic_runner.hpp b/capabilities2_runner/include/capabilities2_runner/topic_runner.hpp index 5cf2138..a053ab9 100644 --- a/capabilities2_runner/include/capabilities2_runner/topic_runner.hpp +++ b/capabilities2_runner/include/capabilities2_runner/topic_runner.hpp @@ -1,9 +1,8 @@ #pragma once + #include -#include "rclcpp/rclcpp.hpp" -#include -#include +#include namespace capabilities2_runner { @@ -14,13 +13,13 @@ namespace capabilities2_runner * Create an topic subsriber for data grabbing capability */ template -class TopicRunner : public RunnerBase +class TopicRunner : public ThreadTriggerRunner { public: /** * @brief Constructor which needs to be empty due to plugin semantics */ - TopicRunner() : RunnerBase() + TopicRunner() : ThreadTriggerRunner() { } @@ -42,93 +41,73 @@ class TopicRunner : public RunnerBase topic_name, 10, [this](const typename TopicT::SharedPtr msg) { this->callback(msg); }); } + /** + * @brief stop function to cease functionality and shutdown + * + */ + virtual void stop(const std::string& bond_id) override + { + // if the node pointer is empty then throw an error + // this means that the runner was not started and is being used out of order + + if (!node_) + throw runner_exception("cannot stop runner that was not started"); + + // throw an error if the service client is null + // this can happen if the runner is not able to find the action resource + + if (!subscription_) + throw runner_exception("cannot stop runner subscriber that was not started"); + + // emit stop event + emit_stopped(bond_id, update_on_stopped(events[runner_id].on_stopped.parameters)); + + RCLCPP_INFO(node_->get_logger(), "runner cleaned. stopping.."); + } + +protected: /** * @brief Trigger process to be executed. * * This method utilizes paramters set via the trigger() function * * @param parameters pointer to tinyxml2::XMLElement that contains parameters + * @param thread_id unique identifier for the execution thread */ - virtual void execution(int id) override + virtual void execution(const std::string& parameters, const std::string& thread_id) override { + // split thread_id to get bond_id and trigger_id (format: "bond_id/trigger_id") + std::string bond_id = ThreadTriggerRunner::bond_from_thread_id(thread_id); + std::string trigger_id = ThreadTriggerRunner::trigger_from_thread_id(thread_id); + // if parameters are not provided then cannot proceed - if (!parameters_[id]) + if (!parameters_[trigger_id]) throw runner_exception("cannot grab data without parameters"); - // trigger the events related to on_started state - if (events[id].on_started.interface != "") - { - event_(EventType::STARTED, id, events[id].on_started.interface, events[id].on_started.provider); - triggerFunction_(events[id].on_started.interface, update_on_started(events[id].on_started.parameters)); - } - + // emit started event + emit_started(bond_id, update_on_started(events[trigger_id].on_started.parameters)); std::unique_lock lock(mutex_); completed_ = false; - info_("Waiting for Message.", id); + RCLCPP_INFO(node_->get_logger(), "Waiting for Message."); // Conditional wait cv_.wait(lock, [this] { return completed_; }); - info_("Message Received.", id); + RCLCPP_INFO(node_->get_logger(), "Message Received."); if (latest_message_) { - // trigger the events related to on_success state - if (events[id].on_success.interface != "") - { - event_(EventType::SUCCEEDED, id, events[id].on_success.interface, events[id].on_success.provider); - triggerFunction_(events[id].on_success.interface, update_on_success(events[id].on_success.parameters)); - } + // emit success event + emit_succeeded(bond_id, update_on_success(events[trigger_id].on_success.parameters)); } else { - error_("Message receving failed."); - - // trigger the events related to on_failure state - if (events[id].on_failure.interface != "") - { - event_(EventType::FAILED, id, events[id].on_failure.interface, events[id].on_failure.provider); - triggerFunction_(events[id].on_failure.interface, update_on_failure(events[id].on_failure.parameters)); - } + RCLCPP_ERROR(node_->get_logger(), "Message receiving failed."); + // emit failed event + emit_failed(bond_id, update_on_failure(events[trigger_id].on_failure.parameters)); } - info_("Thread closing.", id); - } - - /** - * @brief stop function to cease functionality and shutdown - * - */ - virtual void stop() override - { - // if the node pointer is empty then throw an error - // this means that the runner was not started and is being used out of order - - if (!node_) - throw runner_exception("cannot stop runner that was not started"); - - // throw an error if the service client is null - // this can happen if the runner is not able to find the action resource - - if (!subscription_) - throw runner_exception("cannot stop runner subscriber that was not started"); - - // Trigger on_stopped event if defined - if (events[runner_id].on_stopped.interface != "") - { - event_(EventType::STOPPED, -1, events[runner_id].on_stopped.interface, events[runner_id].on_stopped.provider); - triggerFunction_(events[runner_id].on_stopped.interface, - update_on_stopped(events[runner_id].on_stopped.parameters)); - } - - info_("removing event options"); - - // remove all event options for this runner instance - const auto n = events.size(); - events.clear(); - info_("removed event options for " + std::to_string(n) + " runner ids"); - - info_("runner cleaned. stopping.."); + RCLCPP_INFO(node_->get_logger(), "Thread closing."); } protected: @@ -152,4 +131,5 @@ class TopicRunner : public RunnerBase mutable typename TopicT::SharedPtr latest_message_; }; -} // namespace capabilities2_runner \ No newline at end of file + +} // namespace capabilities2_runner diff --git a/capabilities2_runner/package.xml b/capabilities2_runner/package.xml index 18c3089..a9e013f 100644 --- a/capabilities2_runner/package.xml +++ b/capabilities2_runner/package.xml @@ -22,10 +22,8 @@ std_msgs capabilities2_msgs capabilities2_events - tinyxml2_vendor - - uuid + tinyxml2_vendor ament_cmake @@ -48,13 +46,13 @@ providers/InputMultiplexAnyRunner.yaml - + - interfaces/CapabilityGetRunner.yaml + interfaces/GetCapabilitySpecsRunner.yaml - providers/CapabilityGetRunner.yaml + providers/GetCapabilitySpecsRunner.yaml diff --git a/capabilities2_runner/plugins.xml b/capabilities2_runner/plugins.xml index b2e2d7a..bfcb864 100644 --- a/capabilities2_runner/plugins.xml +++ b/capabilities2_runner/plugins.xml @@ -21,9 +21,9 @@ A plugin that provides a dummy-runner to test capabilities2 - + - A plugin that request capabilities from the capabilities server and transfers to an LLM + A plugin that request capabilities from the capabilities server diff --git a/capabilities2_runner/src/capability_get_runner.cpp b/capabilities2_runner/src/capability_get_runner.cpp deleted file mode 100644 index 55a61a7..0000000 --- a/capabilities2_runner/src/capability_get_runner.cpp +++ /dev/null @@ -1,6 +0,0 @@ -#include -#include -#include - -// register runner plugins -PLUGINLIB_EXPORT_CLASS(capabilities2_runner::CapabilityGetRunner, capabilities2_runner::RunnerBase); diff --git a/capabilities2_runner/src/dummy_runner.cpp b/capabilities2_runner/src/dummy_runner.cpp index 2574e51..d7f3626 100644 --- a/capabilities2_runner/src/dummy_runner.cpp +++ b/capabilities2_runner/src/dummy_runner.cpp @@ -13,16 +13,19 @@ namespace capabilities2_runner class DummyRunner : public RunnerBase { public: - void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config) override + void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config, const std::string& bond_id) override { // init the base runner init_base(node, run_config); // do nothing RCLCPP_INFO(node_->get_logger(), "Dummy runner started"); + + // emit started event + emit_started(bond_id, "dummy_parameters"); } - void stop() override + void stop(const std::string& bond_id) override { // guard node if (!node_) @@ -30,18 +33,20 @@ class DummyRunner : public RunnerBase // stop the runner RCLCPP_INFO(node_->get_logger(), "Dummy runner stopped"); - } - void trigger(const std::string& parameters) override - { - RCLCPP_INFO(node_->get_logger(), "Dummy runner cannot trigger"); + // emit stopped event + emit_stopped(bond_id, "dummy_parameters"); } -protected: - // throw on triggerExecution function - void execution(int id) override + void trigger(const std::string& parameters, const std::string& bond_id) override { - RCLCPP_INFO(node_->get_logger(), "Dummy runner does not have triggerExecution()"); + RCLCPP_WARN(node_->get_logger(), "Dummy runner cannot trigger"); + + // emit failed event + emit_failed(bond_id, "Dummy runner cannot trigger"); + + // throw an exception as this runner does not support trigger execution + throw runner_exception("Dummy runner does not support trigger execution"); } }; diff --git a/capabilities2_runner/src/get_capability_specs_runner.cpp b/capabilities2_runner/src/get_capability_specs_runner.cpp new file mode 100644 index 0000000..d14c050 --- /dev/null +++ b/capabilities2_runner/src/get_capability_specs_runner.cpp @@ -0,0 +1,6 @@ +#include +#include +#include + +// register runner plugins +PLUGINLIB_EXPORT_CLASS(capabilities2_runner::GetCapabilitySpecsRunner, capabilities2_runner::RunnerBase); diff --git a/capabilities2_runner/src/launch_runner.cpp b/capabilities2_runner/src/launch_runner.cpp index 53e2844..912983b 100644 --- a/capabilities2_runner/src/launch_runner.cpp +++ b/capabilities2_runner/src/launch_runner.cpp @@ -3,4 +3,4 @@ #include // register runner plugins -PLUGINLIB_EXPORT_CLASS(capabilities2_runner_capabilities::LaunchRunner, capabilities2_runner::RunnerBase) +PLUGINLIB_EXPORT_CLASS(capabilities2_runner::LaunchRunner, capabilities2_runner::RunnerBase) From 1f34bb5cfb9613f4cd82b4f0e7f6f216ea29314c Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Mon, 9 Feb 2026 04:38:50 +0000 Subject: [PATCH 23/37] updated event_node with capability_options --- .../capabilities2_events/event_node.hpp | 6 +- capabilities2_msgs/CMakeLists.txt | 1 + capabilities2_msgs/msg/Capability.msg | 4 +- capabilities2_msgs/msg/CapabilityOption.msg | 20 ++ .../CapabilityOptions.hpp | 245 ++++++++++++++++++ .../capabilities2_runner/runner_base.hpp | 111 ++++---- 6 files changed, 327 insertions(+), 60 deletions(-) create mode 100644 capabilities2_msgs/msg/CapabilityOption.msg create mode 100644 capabilities2_runner/include/capabilities2_runner/CapabilityOptions.hpp diff --git a/capabilities2_events/include/capabilities2_events/event_node.hpp b/capabilities2_events/include/capabilities2_events/event_node.hpp index a101b1e..d9643ca 100644 --- a/capabilities2_events/include/capabilities2_events/event_node.hpp +++ b/capabilities2_events/include/capabilities2_events/event_node.hpp @@ -9,6 +9,7 @@ #include #include +#include #include #include @@ -61,7 +62,8 @@ class EventNode * @param event_type * @param parameters */ - void emit_event(const std::string& bond_id, const uint8_t& event_type, const std::string& parameters) + void emit_event(const std::string& bond_id, const capabilities2_msgs::msg::CapabilityEventCode& event_type, + const capabilities2::CapabilityOptions& parameters) { // check if event emitter is set if (!event_emitter_) @@ -84,7 +86,7 @@ class EventNode // parameterise target capability // create a copy of target with updated parameters capabilities2_msgs::msg::Capability target_with_params = connection.target; - target_with_params.parameters = parameters; + target_with_params.parameters = parameters.toMsg().parameters; // emit event via event api // NOTE: callback invocation is handled by event api diff --git a/capabilities2_msgs/CMakeLists.txt b/capabilities2_msgs/CMakeLists.txt index 0971271..eb9f46f 100644 --- a/capabilities2_msgs/CMakeLists.txt +++ b/capabilities2_msgs/CMakeLists.txt @@ -27,6 +27,7 @@ set(msg_files "msg/NaturalCapability.msg" "msg/Remapping.msg" "msg/RunningCapability.msg" + "msg/CapabilityOption.msg" ) set(srv_files diff --git a/capabilities2_msgs/msg/Capability.msg b/capabilities2_msgs/msg/Capability.msg index 5bf97a1..202c267 100644 --- a/capabilities2_msgs/msg/Capability.msg +++ b/capabilities2_msgs/msg/Capability.msg @@ -4,5 +4,5 @@ string capability # Used provider string provider -# trigger parameters -string parameters +# Trigger parameters +CapabilityOption[] parameters \ No newline at end of file diff --git a/capabilities2_msgs/msg/CapabilityOption.msg b/capabilities2_msgs/msg/CapabilityOption.msg new file mode 100644 index 0000000..e14ef44 --- /dev/null +++ b/capabilities2_msgs/msg/CapabilityOption.msg @@ -0,0 +1,20 @@ +# supported types of option +uint8 BOOL = 0 +uint8 DOUBLE = 1 +uint8 INT = 2 +uint8 STRING = 3 +uint8 VECTOR_BOOL = 4 +uint8 VECTOR_DOUBLE = 5 +uint8 VECTOR_INT = 6 +uint8 VECTOR_STRING = 7 + + +# Key of the option +string key + +# Value array of option for vector types. +# insert a single value for non-vector types. +string[] value + +# Type of the option +uint8 type \ No newline at end of file diff --git a/capabilities2_runner/include/capabilities2_runner/CapabilityOptions.hpp b/capabilities2_runner/include/capabilities2_runner/CapabilityOptions.hpp new file mode 100644 index 0000000..86cfb04 --- /dev/null +++ b/capabilities2_runner/include/capabilities2_runner/CapabilityOptions.hpp @@ -0,0 +1,245 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +namespace capabilities2 +{ + +struct options_exception : public std::runtime_error +{ + using std::runtime_error::runtime_error; + + options_exception(const std::string& what) : std::runtime_error(what) + { + } + + virtual const char* what() const noexcept override + { + return std::runtime_error::what(); + } +}; + +enum class OptionType +{ + BOOL, + DOUBLE, + INT, + STRING, + VECTOR_BOOL, + VECTOR_DOUBLE, + VECTOR_INT, + VECTOR_STRING +}; + +/** + * @brief Key value pair for capability options + * + * @param key the key of the option + * @param value the value of the option + */ +struct Options +{ + std::string key; + std::vector value; + OptionType type; + + capabilities2_msgs::msg::CapabilityOption toMsg() const + { + capabilities2_msgs::msg::CapabilityOption msg; + msg.key = key; + msg.value = value; + msg.type = static_cast(type); + return msg; + } + + void fromMsg(const capabilities2_msgs::msg::CapabilityOption& msg) + { + key = msg.key; + value = msg.value; + type = static_cast(msg.type); + } +}; + +/** + * @brief capability options for a capability runner given by interface and provider + * + * @param interface the interface of the capability + * @param provider the provider of the capability + * @param options the options for the capability + */ +struct CapabilityOptions +{ + std::string interface; + std::string provider; + std::vector options; + + std::any get_value(const std::string& key) const + { + for (const auto& option : options) + if (option.key == key) + { + try + { + switch (option.type) + { + case OptionType::BOOL: + return option.value[0] == "true"; + case OptionType::DOUBLE: + return std::stod(option.value[0]); + case OptionType::INT: + return std::stoi(option.value[0]); + case OptionType::STRING: + return option.value[0]; + case OptionType::VECTOR_BOOL: { + std::vector vec; + for (const auto& v : option.value) + vec.push_back(v == "true"); + return vec; + } + case OptionType::VECTOR_DOUBLE: { + std::vector vec; + for (const auto& v : option.value) + vec.push_back(std::stod(v)); + return vec; + } + case OptionType::VECTOR_INT: { + std::vector vec; + for (const auto& v : option.value) + vec.push_back(std::stoi(v)); + return vec; + } + case OptionType::VECTOR_STRING: + return option.value; + } + } + catch (const std::exception& e) + { + throw options_exception("Failed to convert option '" + option.key + "': " + e.what()); + } + + throw options_exception("Unknown option type for key: " + option.key); + } + } + + void set_value(const std::string& key, const OptionType& type, const std::any& value) + { + for (auto& option : options) + if (option.key == key) + { + switch (type) + { + case OptionType::BOOL: + option.value[0] = std::any_cast(value) ? "true" : "false"; + return; + case OptionType::DOUBLE: + option.value[0] = std::to_string(std::any_cast(value)); + return; + case OptionType::INT: + option.value[0] = std::to_string(std::any_cast(value)); + return; + case OptionType::STRING: + option.value[0] = std::any_cast(value); + return; + case OptionType::VECTOR_BOOL: { + const auto& vec = std::any_cast>(value); + option.value.clear(); + for (const auto& v : vec) + option.value.push_back(v ? "true" : "false"); + return; + } + case OptionType::VECTOR_DOUBLE: { + const auto& vec = std::any_cast>(value); + option.value.clear(); + for (const auto& v : vec) + option.value.push_back(std::to_string(v)); + return; + } + case OptionType::VECTOR_INT: { + const auto& vec = std::any_cast>(value); + option.value.clear(); + for (const auto& v : vec) + option.value.push_back(std::to_string(v)); + return; + } + case OptionType::VECTOR_STRING: + option.value = std::any_cast>(value); + return; + } + } + + // Option not found, add new one + Options new_option; + new_option.key = key; + new_option.type = type; + + switch (type) + { + case OptionType::BOOL: + new_option.value[0] = std::any_cast(value) ? "true" : "false"; + break; + case OptionType::DOUBLE: + new_option.value[0] = std::to_string(std::any_cast(value)); + break; + case OptionType::INT: + new_option.value[0] = std::to_string(std::any_cast(value)); + break; + case OptionType::STRING: + new_option.value[0] = std::any_cast(value); + break; + case OptionType::VECTOR_BOOL: { + const auto& vec = std::any_cast>(value); + for (const auto& v : vec) + new_option.value.push_back(v ? "true" : "false"); + break; + } + case OptionType::VECTOR_DOUBLE: { + const auto& vec = std::any_cast>(value); + for (const auto& v : vec) + new_option.value.push_back(std::to_string(v)); + break; + } + case OptionType::VECTOR_INT: { + const auto& vec = std::any_cast>(value); + for (const auto& v : vec) + new_option.value.push_back(std::to_string(v)); + break; + } + case OptionType::VECTOR_STRING: + new_option.value = std::any_cast>(value); + break; + }; + + options.push_back(new_option); + } + + capabilities2_msgs::msg::Capability toMsg() const + { + capabilities2_msgs::msg::Capability msg; + msg.interface = interface; + msg.provider = provider; + for (const auto& option : options) + msg.options.push_back(option.toMsg()); + return msg; + } + + void fromMsg(const capabilities2_msgs::msg::Capability& msg) + { + interface = msg.interface; + provider = msg.provider; + options.clear(); + for (const auto& option_msg : msg.options) + { + Options option; + option.fromMsg(option_msg); + options.push_back(option); + } + } +}; + +} // namespace capabilities2 \ No newline at end of file diff --git a/capabilities2_runner/include/capabilities2_runner/runner_base.hpp b/capabilities2_runner/include/capabilities2_runner/runner_base.hpp index 1ab4c38..7971a1a 100644 --- a/capabilities2_runner/include/capabilities2_runner/runner_base.hpp +++ b/capabilities2_runner/include/capabilities2_runner/runner_base.hpp @@ -9,6 +9,7 @@ #include #include +#include namespace capabilities2_runner { @@ -82,22 +83,22 @@ class RunnerBase : public capabilities2_events::EventNode * * @return `true` if element is not nullptr and conversion successful, `false` if element is nullptr */ - static const std::string convert_to_string(tinyxml2::XMLElement* element) - { - if (element) - { - tinyxml2::XMLPrinter printer; - - element->Accept(&printer); - std::string parameters = printer.CStr(); - return parameters; - } - else - { - std::string parameters = ""; - return parameters; - } - } + // static const std::string convert_to_string(tinyxml2::XMLElement* element) + // { + // if (element) + // { + // tinyxml2::XMLPrinter printer; + + // element->Accept(&printer); + // std::string parameters = printer.CStr(); + // return parameters; + // } + // else + // { + // std::string parameters = ""; + // return parameters; + // } + // } /** * @brief convert an XMLElement to std::string @@ -107,21 +108,21 @@ class RunnerBase : public capabilities2_events::EventNode * * @return `true` if element is not nullptr and conversion successful, `false` if element is nullptr */ - static tinyxml2::XMLElement* convert_to_xml(const std::string& parameters) - { - tinyxml2::XMLDocument doc; - - if (parameters != "") - { - doc.Parse(parameters.c_str()); - tinyxml2::XMLElement* element = doc.FirstChildElement(); - return element; - } - else - { - return nullptr; - } - } + // static tinyxml2::XMLElement* convert_to_xml(const std::string& parameters) + // { + // tinyxml2::XMLDocument doc; + + // if (parameters != "") + // { + // doc.Parse(parameters.c_str()); + // tinyxml2::XMLElement* element = doc.FirstChildElement(); + // return element; + // } + // else + // { + // return nullptr; + // } + // } public: RunnerBase() : run_config_(), execution_should_stop_(false) @@ -169,7 +170,7 @@ class RunnerBase : public capabilities2_events::EventNode * @param bond_id unique identifier for the group of connections associated with this runner trigger event * */ - virtual void trigger(const std::string& parameters, const std::string& bond_id) + virtual void trigger(const capabilities2::CapabilityOptions& parameters, const std::string& bond_id) { // TODO: verify parameter formatting (safe xml string or other format) // TODO: minimum parameter set? @@ -354,7 +355,7 @@ class RunnerBase : public capabilities2_events::EventNode * * NOTE: should call success and failure events appropriately */ - virtual void execution(const std::string& parameters, const std::string& bond_id) = 0; + virtual void execution(const capabilities2::CapabilityOptions& parameters, const std::string& bond_id) = 0; /** */ void stop_execution(std::chrono::milliseconds timeout = std::chrono::milliseconds(500)) @@ -392,15 +393,13 @@ class RunnerBase : public capabilities2_events::EventNode /** * @brief Update on_started event parameters with new data if available. * - * This function is used to inject new data into the XMLElement containing + * This function is used to inject new data into the CapabilityOptions containing * parameters related to the on_started trigger event * - * A pattern needs to be implemented in the derived class - * - * @param parameters pointer to the XMLElement containing parameters - * @return pointer to the XMLElement containing updated parameters + * @param parameters CapabilityOptions containing parameters + * @return CapabilityOptions containing updated parameters */ - virtual std::string update_on_started(std::string& parameters) + virtual capabilities2::CapabilityOptions update_on_started(capabilities2::CapabilityOptions& parameters) { return parameters; }; @@ -408,15 +407,15 @@ class RunnerBase : public capabilities2_events::EventNode /** * @brief Update on_stopped event parameters with new data if available. * - * This function is used to inject new data into the XMLElement containing + * This function is used to inject new data into the CapabilityOptions containing * parameters related to the on_stopped trigger event * * A pattern needs to be implemented in the derived class * - * @param parameters pointer to the XMLElement containing parameters - * @return pointer to the XMLElement containing updated parameters + * @param parameters CapabilityOptions containing parameters + * @return CapabilityOptions containing updated parameters */ - virtual std::string update_on_stopped(std::string& parameters) + virtual capabilities2::CapabilityOptions update_on_stopped(capabilities2::CapabilityOptions& parameters) { return parameters; }; @@ -424,15 +423,15 @@ class RunnerBase : public capabilities2_events::EventNode /** * @brief Update on_failure event parameters with new data if available. * - * This function is used to inject new data into the XMLElement containing + * This function is used to inject new data into the CapabilityOptions containing * parameters related to the on_failure trigger event * * A pattern needs to be implemented in the derived class * - * @param parameters pointer to the XMLElement containing parameters - * @return pointer to the XMLElement containing updated parameters + * @param parameters CapabilityOptions containing parameters + * @return CapabilityOptions containing updated parameters */ - virtual std::string update_on_failure(std::string& parameters) + virtual capabilities2::CapabilityOptions update_on_failure(capabilities2::CapabilityOptions& parameters) { return parameters; }; @@ -440,15 +439,15 @@ class RunnerBase : public capabilities2_events::EventNode /** * @brief Update on_success event parameters with new data if available. * - * This function is used to inject new data into the XMLElement containing + * This function is used to inject new data into the CapabilityOptions containing * parameters related to the on_success trigger event * * A pattern needs to be implemented in the derived class * - * @param parameters pointer to the XMLElement containing parameters - * @return pointer to the XMLElement containing updated parameters + * @param parameters CapabilityOptions containing parameters + * @return CapabilityOptions containing updated parameters */ - virtual std::string update_on_success(std::string& parameters) + virtual capabilities2::CapabilityOptions update_on_success(capabilities2::CapabilityOptions& parameters) { return parameters; }; @@ -462,7 +461,7 @@ class RunnerBase : public capabilities2_events::EventNode * * WARNING: this only gets the first resource found of the given type * - * @param resource_type + * @param resource_type * @param msg_type * @return const std::string */ @@ -596,7 +595,7 @@ class RunnerBase : public capabilities2_events::EventNode * @param bond_id * @param parameters */ - void emit_started(const std::string& bond_id, const std::string& parameters = "") + void emit_started(const std::string& bond_id, const capabilities2::CapabilityOptions& parameters = capabilities2::CapabilityOptions()) { capabilities2_msgs::msg::CapabilityEventCode event_type; event_type.code = capabilities2_msgs::msg::CapabilityEventCode::STARTED; @@ -609,7 +608,7 @@ class RunnerBase : public capabilities2_events::EventNode * @param bond_id * @param parameters */ - void emit_stopped(const std::string& bond_id, const std::string& parameters = "") + void emit_stopped(const std::string& bond_id, const capabilities2::CapabilityOptions& parameters = capabilities2::CapabilityOptions()) { capabilities2_msgs::msg::CapabilityEventCode event_type; event_type.code = capabilities2_msgs::msg::CapabilityEventCode::STOPPED; @@ -622,7 +621,7 @@ class RunnerBase : public capabilities2_events::EventNode * @param bond_id * @param parameters */ - void emit_succeeded(const std::string& bond_id, const std::string& parameters = "") + void emit_succeeded(const std::string& bond_id, const capabilities2::CapabilityOptions& parameters = capabilities2::CapabilityOptions()) { capabilities2_msgs::msg::CapabilityEventCode event_type; event_type.code = capabilities2_msgs::msg::CapabilityEventCode::SUCCEEDED; @@ -635,7 +634,7 @@ class RunnerBase : public capabilities2_events::EventNode * @param bond_id * @param parameters */ - void emit_failed(const std::string& bond_id, const std::string& parameters = "") + void emit_failed(const std::string& bond_id, const capabilities2::CapabilityOptions& parameters = capabilities2::CapabilityOptions()) { capabilities2_msgs::msg::CapabilityEventCode event_type; event_type.code = capabilities2_msgs::msg::CapabilityEventCode::FAILED; From 2c0246f700cbff0cb555980d07cc5efdb14f0d45 Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Mon, 9 Feb 2026 07:07:17 +0000 Subject: [PATCH 24/37] removed tinyxml2 from runner dependencies --- .../capabilities2_events/event_node.hpp | 19 ++--- capabilities2_runner/CMakeLists.txt | 9 +-- .../CapabilityOptions.hpp | 0 .../capabilities2_runner/action_runner.hpp | 25 ++++--- .../capabilities2_runner/encap_runner.hpp | 0 .../capabilities2_runner/launch_runner.hpp | 0 .../multiplex_base_runner.hpp | 2 +- .../capabilities2_runner/notrigger_runner.hpp | 2 +- .../capabilities2_runner/runner_base.hpp | 71 +++++++------------ .../capabilities2_runner/service_runner.hpp | 13 ++-- .../system/get_capability_specs_runner.hpp | 0 .../system/input_multiplex_all_runner.hpp | 0 .../system/input_multiplex_any_runner.hpp | 0 .../threadtrigger_runner.hpp | 16 +++-- .../capabilities2_runner/topic_runner.hpp | 2 +- capabilities2_runner/src/dummy_runner.cpp | 2 +- .../get_capability_specs_runner.cpp | 0 .../src/{ => system}/multiplex_all_runner.cpp | 0 .../src/{ => system}/multiplex_any_runner.cpp | 0 .../capabilities2_server/capabilities_api.hpp | 2 +- .../capabilities_server.hpp | 1 - .../capabilities2_server/runner_cache.hpp | 2 +- 22 files changed, 72 insertions(+), 94 deletions(-) rename capabilities2_runner/{include => docs}/capabilities2_runner/CapabilityOptions.hpp (100%) rename capabilities2_runner/{include => docs}/capabilities2_runner/action_runner.hpp (89%) rename capabilities2_runner/{include => docs}/capabilities2_runner/encap_runner.hpp (100%) rename capabilities2_runner/{include => docs}/capabilities2_runner/launch_runner.hpp (100%) rename capabilities2_runner/{include => docs}/capabilities2_runner/multiplex_base_runner.hpp (96%) rename capabilities2_runner/{include => docs}/capabilities2_runner/notrigger_runner.hpp (84%) rename capabilities2_runner/{include => docs}/capabilities2_runner/runner_base.hpp (83%) rename capabilities2_runner/{include => docs}/capabilities2_runner/service_runner.hpp (86%) rename capabilities2_runner/{include => docs}/capabilities2_runner/system/get_capability_specs_runner.hpp (100%) rename capabilities2_runner/{include => docs}/capabilities2_runner/system/input_multiplex_all_runner.hpp (100%) rename capabilities2_runner/{include => docs}/capabilities2_runner/system/input_multiplex_any_runner.hpp (100%) rename capabilities2_runner/{include => docs}/capabilities2_runner/threadtrigger_runner.hpp (86%) rename capabilities2_runner/{include => docs}/capabilities2_runner/topic_runner.hpp (97%) rename capabilities2_runner/src/{ => system}/get_capability_specs_runner.cpp (100%) rename capabilities2_runner/src/{ => system}/multiplex_all_runner.cpp (100%) rename capabilities2_runner/src/{ => system}/multiplex_any_runner.cpp (100%) diff --git a/capabilities2_events/include/capabilities2_events/event_node.hpp b/capabilities2_events/include/capabilities2_events/event_node.hpp index d9643ca..e39d550 100644 --- a/capabilities2_events/include/capabilities2_events/event_node.hpp +++ b/capabilities2_events/include/capabilities2_events/event_node.hpp @@ -124,14 +124,16 @@ class EventNode } /** - * @brief Add a new connection + * @brief make EventNode::add_connection public method on runner api. + * + * add connection to this runner to target capability this allows the runner to emit events on state changes + * to the target capability the connection ID format is: "bond_id/trigger_id" which allows event emission to + * extract bond_id for access control * - * from this event node to a target capability with a given event type - * - * @param connection_id - * @param type - * @param target - * @param event_cb + * @param connection_id unique identifier for the connection (format: "bond_id/trigger_id") + * @param type type of event to connect to + * @param target target capability to connect to + * @param callback callback to trigger target capability with (capability, parameters, bond_id) * * @throws event_exception if connection with given id already exists */ @@ -262,8 +264,7 @@ class EventNode // source capability of this event node capabilities2_msgs::msg::Capability source_; - // event emitter - // used to emit events + // event emitter used to emit events // store as member to avoid passing around std::shared_ptr event_emitter_; diff --git a/capabilities2_runner/CMakeLists.txt b/capabilities2_runner/CMakeLists.txt index a7d4d80..e4b7d79 100644 --- a/capabilities2_runner/CMakeLists.txt +++ b/capabilities2_runner/CMakeLists.txt @@ -16,8 +16,6 @@ find_package(rclcpp_action REQUIRED) find_package(pluginlib REQUIRED) find_package(capabilities2_msgs REQUIRED) find_package(capabilities2_events REQUIRED) -find_package(tinyxml2_vendor REQUIRED) -find_package(TinyXML2 REQUIRED) # provided by tinyxml2 upstream, or tinyxml2_vendor include_directories( include @@ -26,9 +24,9 @@ include_directories( add_library(${PROJECT_NAME} SHARED src/dummy_runner.cpp src/launch_runner.cpp - src/get_capability_specs_runner.cpp - src/multiplex_all_runner.cpp - src/multiplex_any_runner.cpp + src/system/get_capability_specs_runner.cpp + src/system/multiplex_all_runner.cpp + src/system/multiplex_any_runner.cpp ) ament_target_dependencies(${PROJECT_NAME} @@ -37,7 +35,6 @@ ament_target_dependencies(${PROJECT_NAME} pluginlib capabilities2_msgs capabilities2_events - TinyXML2 ) pluginlib_export_plugin_description_file(${PROJECT_NAME} plugins.xml) diff --git a/capabilities2_runner/include/capabilities2_runner/CapabilityOptions.hpp b/capabilities2_runner/docs/capabilities2_runner/CapabilityOptions.hpp similarity index 100% rename from capabilities2_runner/include/capabilities2_runner/CapabilityOptions.hpp rename to capabilities2_runner/docs/capabilities2_runner/CapabilityOptions.hpp diff --git a/capabilities2_runner/include/capabilities2_runner/action_runner.hpp b/capabilities2_runner/docs/capabilities2_runner/action_runner.hpp similarity index 89% rename from capabilities2_runner/include/capabilities2_runner/action_runner.hpp rename to capabilities2_runner/docs/capabilities2_runner/action_runner.hpp index 42d4fd8..2d80bc7 100644 --- a/capabilities2_runner/include/capabilities2_runner/action_runner.hpp +++ b/capabilities2_runner/docs/capabilities2_runner/action_runner.hpp @@ -119,20 +119,16 @@ class ActionRunner : public ThreadTriggerRunner * * This method utilizes paramters set via the trigger() function * - * @param parameters pointer to tinyxml2::XMLElement that contains parameters + * @param parameters pointer to capabilities2::CapabilityOptions that contains parameters */ - virtual void execution(const std::string& parameters, const std::string& thread_id) override + virtual void execution(const capabilities2::CapabilityOptions& parameters, const std::string& thread_id) override { // split thread_id to get bond_id and trigger_id (format: "bond_id/trigger_id") std::string bond_id = ThreadTriggerRunner::bond_from_thread_id(thread_id); std::string trigger_id = ThreadTriggerRunner::trigger_from_thread_id(thread_id); - // if parameters are not provided then cannot proceed - if (!parameters_[trigger_id]) - throw runner_exception("cannot trigger action without parameters"); - - // generate a goal from parameters if provided - goal_msg_ = generate_goal(parameters_[trigger_id], trigger_id); + // generate a goal from parameters provided + goal_msg_ = generate_goal(parameters, trigger_id); RCLCPP_INFO(node_->get_logger(), "goal generated for event " + trigger_id); std::unique_lock lock(mutex_); @@ -207,10 +203,12 @@ class ActionRunner : public ThreadTriggerRunner * * A pattern needs to be implemented in the derived class * - * @param parameters + * @param parameters capability options that contain parameters for the trigger + * @param trigger_id the trigger_id associated with this execution thread * @return ActionT::Goal the generated goal */ - virtual typename ActionT::Goal generate_goal(tinyxml2::XMLElement* parameters, const std::string& trigger_id) = 0; + virtual typename ActionT::Goal generate_goal(const capabilities2::CapabilityOptions parameters, + const std::string& trigger_id) = 0; /** * @brief Generate a std::string from feedback message @@ -220,11 +218,12 @@ class ActionRunner : public ThreadTriggerRunner * A pattern needs to be implemented in the derived class. If the feedback string * is empty, nothing will be printed on the screen * - * @param parameters + * @param msg the feedback message received from the action server + * @param trigger_id the trigger_id associated with this feedback message * @return ActionT::Feedback the received feedback */ - virtual std::string generate_feedback(const typename ActionT::Feedback::ConstSharedPtr msg, - const std::string& trigger_id) = 0; + virtual capabilities2::CapabilityOptions generate_feedback(const typename ActionT::Feedback::ConstSharedPtr msg, + const std::string& trigger_id) = 0; protected: /**< action client */ diff --git a/capabilities2_runner/include/capabilities2_runner/encap_runner.hpp b/capabilities2_runner/docs/capabilities2_runner/encap_runner.hpp similarity index 100% rename from capabilities2_runner/include/capabilities2_runner/encap_runner.hpp rename to capabilities2_runner/docs/capabilities2_runner/encap_runner.hpp diff --git a/capabilities2_runner/include/capabilities2_runner/launch_runner.hpp b/capabilities2_runner/docs/capabilities2_runner/launch_runner.hpp similarity index 100% rename from capabilities2_runner/include/capabilities2_runner/launch_runner.hpp rename to capabilities2_runner/docs/capabilities2_runner/launch_runner.hpp diff --git a/capabilities2_runner/include/capabilities2_runner/multiplex_base_runner.hpp b/capabilities2_runner/docs/capabilities2_runner/multiplex_base_runner.hpp similarity index 96% rename from capabilities2_runner/include/capabilities2_runner/multiplex_base_runner.hpp rename to capabilities2_runner/docs/capabilities2_runner/multiplex_base_runner.hpp index 86f85e7..b6d8c1f 100644 --- a/capabilities2_runner/include/capabilities2_runner/multiplex_base_runner.hpp +++ b/capabilities2_runner/docs/capabilities2_runner/multiplex_base_runner.hpp @@ -59,7 +59,7 @@ class MultiplexBaseRunner : public ThreadTriggerRunner * * @param id unique identifier for the execution */ - virtual void execution(const std::string& parameters, const std::string& thread_id) override + virtual void execution(const capabilities2::CapabilityOptions& parameters, const std::string& thread_id) override { // extract trigger_id from thread_id (format: "bond_id/trigger_id") size_t slash_pos = thread_id.find('/'); diff --git a/capabilities2_runner/include/capabilities2_runner/notrigger_runner.hpp b/capabilities2_runner/docs/capabilities2_runner/notrigger_runner.hpp similarity index 84% rename from capabilities2_runner/include/capabilities2_runner/notrigger_runner.hpp rename to capabilities2_runner/docs/capabilities2_runner/notrigger_runner.hpp index 6c0bea0..63c4c05 100644 --- a/capabilities2_runner/include/capabilities2_runner/notrigger_runner.hpp +++ b/capabilities2_runner/docs/capabilities2_runner/notrigger_runner.hpp @@ -16,7 +16,7 @@ class NoTriggerRunner : public RunnerBase { public: // throw on trigger function - void trigger(const std::string& parameters, const std::string& bond_id) override + void trigger(const capabilities2::CapabilityOptions& parameters, const std::string& bond_id) override { // emit failed event emit_failed(bond_id, "cannot trigger this is a no-trigger runner"); diff --git a/capabilities2_runner/include/capabilities2_runner/runner_base.hpp b/capabilities2_runner/docs/capabilities2_runner/runner_base.hpp similarity index 83% rename from capabilities2_runner/include/capabilities2_runner/runner_base.hpp rename to capabilities2_runner/docs/capabilities2_runner/runner_base.hpp index 52b3a75..6a0303a 100644 --- a/capabilities2_runner/include/capabilities2_runner/runner_base.hpp +++ b/capabilities2_runner/docs/capabilities2_runner/runner_base.hpp @@ -67,8 +67,7 @@ struct runner_opts /** * @brief base class for all runners * - * Defines the runner plugin api - * inherits from EventNode to provide event emission + * Defines the runner plugin api. Inherits from EventNode to provide event emission * */ class RunnerBase : public capabilities2_events::EventNode @@ -90,52 +89,29 @@ class RunnerBase : public capabilities2_events::EventNode * @param node shared pointer to the capabilities node. Allows to use ros node related functionalities * @param run_config runner configuration loaded from the yaml file * - * NOTE: must call init_base in derived class implementation - * NOTE: should call start event + * @attention Must call init_base in derived class implementation and should call start event */ virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config, const std::string& bond_id) = 0; /** * @brief stop the runner * - * NOTE: should clean up threads - * NOTE: should call stop event + * @attention should clean up threads and should call stop event */ virtual void stop(const std::string& bond_id) = 0; /** * FIXME: implement new event subsystem - * * @brief Trigger the runner * * This method allows insertion of parameters in a runner after it has been initialized. it is an approach * to parameterise capabilities. Internally starts up RunnerBase::triggerExecution in a thread * - * @param parameters pointer to tinyxml2::XMLElement that contains parameters + * @param parameters capability options that contain parameters for the trigger * @param bond_id unique identifier for the group of connections associated with this runner trigger event * */ - // TODO: verify parameter formatting (safe xml string or other format) - // TODO: minimum parameter set? - - // DEPRECATED: extract trigger id from parameters - // // extract the unique id for the runner and use that as the thread id - // tinyxml2::XMLElement* element = nullptr; - // element = convert_to_xml(parameters); - // if (!element) - // { - // // when this is empty it means that the trigger activation was performed - // // by a registered user - // // this is valid but we need to get a unique trigger id for the runner - // // to proceed safely - // RCLCPP_WARN(node_->get_logger(), "no trigger parameters provided"); - // } - - // std::string trigger_id = ""; - // element->QueryStringAttribute("id", &trigger_id); - - // parameters_[trigger_id] = element; - virtual void trigger(const std::string& parameters, const std::string& bond_id) = 0; + virtual void trigger(const capabilities2::CapabilityOptions& parameters, const std::string& bond_id) = 0; /** * @brief Initializer function for initializing the base runner in place of constructor due to plugin semantics @@ -170,22 +146,21 @@ class RunnerBase : public capabilities2_events::EventNode } /** - * @brief make EventNode::add_connection public method on runner api - * - * @param connection_id - * @param type - * @param target - * @param callback + * @brief make EventNode::add_connection public method on runner api. + * + * add connection to this runner to target capability this allows the runner to emit events on state changes + * to the target capability the connection ID format is: "bond_id/trigger_id" which allows event emission to + * extract bond_id for access control + * + * @param connection_id unique identifier for the connection (format: "bond_id/trigger_id") + * @param type type of event to connect to + * @param target target capability to connect to + * @param callback callback to trigger target capability with (capability, parameters, bond_id) */ void add_connection(const std::string& connection_id, const capabilities2_msgs::msg::CapabilityEventCode& type, const capabilities2_msgs::msg::Capability& target, std::function callback) { - // add connection to this runner to target capability - // this allows the runner to emit events on state changes - // to the target capability - // the connection ID format is: "bond_id/trigger_id" - // which allows event emission to extract bond_id for access control EventNode::add_connection(connection_id, type, target, callback); } @@ -231,8 +206,6 @@ class RunnerBase : public capabilities2_events::EventNode protected: // FIXME: implement new event subsystem - // STATE CHANGE PARAMETER HELPERS - /** * @brief Update on_started event parameters with new data if available. * @@ -304,7 +277,7 @@ class RunnerBase : public capabilities2_events::EventNode * * WARNING: this only gets the first resource found of the given type * - * @param resource_type + * @param resource_type * @param msg_type * @return const std::string */ @@ -437,7 +410,8 @@ class RunnerBase : public capabilities2_events::EventNode * @param bond_id * @param parameters */ - void emit_started(const std::string& bond_id, const capabilities2::CapabilityOptions& parameters = capabilities2::CapabilityOptions()) + void emit_started(const std::string& bond_id, + const capabilities2::CapabilityOptions& parameters = capabilities2::CapabilityOptions()) { emit_event(bond_id, capabilities2_msgs::msg::CapabilityEventCode::STARTED, parameters); } @@ -448,7 +422,8 @@ class RunnerBase : public capabilities2_events::EventNode * @param bond_id * @param parameters */ - void emit_stopped(const std::string& bond_id, const capabilities2::CapabilityOptions& parameters = capabilities2::CapabilityOptions()) + void emit_stopped(const std::string& bond_id, + const capabilities2::CapabilityOptions& parameters = capabilities2::CapabilityOptions()) { emit_event(bond_id, capabilities2_msgs::msg::CapabilityEventCode::STOPPED, parameters); } @@ -459,7 +434,8 @@ class RunnerBase : public capabilities2_events::EventNode * @param bond_id * @param parameters */ - void emit_succeeded(const std::string& bond_id, const capabilities2::CapabilityOptions& parameters = capabilities2::CapabilityOptions()) + void emit_succeeded(const std::string& bond_id, + const capabilities2::CapabilityOptions& parameters = capabilities2::CapabilityOptions()) { emit_event(bond_id, capabilities2_msgs::msg::CapabilityEventCode::SUCCEEDED, parameters); } @@ -470,7 +446,8 @@ class RunnerBase : public capabilities2_events::EventNode * @param bond_id * @param parameters */ - void emit_failed(const std::string& bond_id, const capabilities2::CapabilityOptions& parameters = capabilities2::CapabilityOptions()) + void emit_failed(const std::string& bond_id, + const capabilities2::CapabilityOptions& parameters = capabilities2::CapabilityOptions()) { emit_event(bond_id, capabilities2_msgs::msg::CapabilityEventCode::FAILED, parameters); } diff --git a/capabilities2_runner/include/capabilities2_runner/service_runner.hpp b/capabilities2_runner/docs/capabilities2_runner/service_runner.hpp similarity index 86% rename from capabilities2_runner/include/capabilities2_runner/service_runner.hpp rename to capabilities2_runner/docs/capabilities2_runner/service_runner.hpp index 0821595..cd004dc 100644 --- a/capabilities2_runner/include/capabilities2_runner/service_runner.hpp +++ b/capabilities2_runner/docs/capabilities2_runner/service_runner.hpp @@ -84,7 +84,7 @@ class ServiceRunner : public ThreadTriggerRunner * @param parameters pointer to tinyxml2::XMLElement that contains parameters * @param thread_id unique identifier for the execution thread */ - virtual void execution(const std::string& parameters, const std::string& thread_id) override + virtual void execution(const capabilities2::CapabilityOptions& parameters, const std::string& thread_id) override { // split thread_id to get bond_id and trigger_id (format: "bond_id/trigger_id") std::string bond_id = ThreadTriggerRunner::bond_from_thread_id(thread_id); @@ -144,15 +144,18 @@ class ServiceRunner : public ThreadTriggerRunner * @param parameters * @return ServiceT::Request the generated request */ - virtual typename ServiceT::Request generate_request(const std::string& parameters, const std::string& trigger_id) = 0; + virtual typename ServiceT::Request generate_request(const capabilities2::CapabilityOptions& parameters, const std::string& trigger_id) = 0; /** * @brief Process the reponse and print data as required * - * @param response service reponse - * @param trigger_id thread id + * @param response service reponse message + * @param trigger_id thread id associated with this response used for logging and event emission + * @return capabilities2::CapabilityOptions containing updated parameters for the on_success event if needed + * + * A pattern needs to be implemented in the derived class for processing the response and extracting data if needed, currently does nothing. */ - virtual void process_response(typename ServiceT::Response::SharedPtr response, const std::string& trigger_id) + virtual capabilities2::CapabilityOptions process_response(typename ServiceT::Response::SharedPtr response, const std::string& trigger_id) { } diff --git a/capabilities2_runner/include/capabilities2_runner/system/get_capability_specs_runner.hpp b/capabilities2_runner/docs/capabilities2_runner/system/get_capability_specs_runner.hpp similarity index 100% rename from capabilities2_runner/include/capabilities2_runner/system/get_capability_specs_runner.hpp rename to capabilities2_runner/docs/capabilities2_runner/system/get_capability_specs_runner.hpp diff --git a/capabilities2_runner/include/capabilities2_runner/system/input_multiplex_all_runner.hpp b/capabilities2_runner/docs/capabilities2_runner/system/input_multiplex_all_runner.hpp similarity index 100% rename from capabilities2_runner/include/capabilities2_runner/system/input_multiplex_all_runner.hpp rename to capabilities2_runner/docs/capabilities2_runner/system/input_multiplex_all_runner.hpp diff --git a/capabilities2_runner/include/capabilities2_runner/system/input_multiplex_any_runner.hpp b/capabilities2_runner/docs/capabilities2_runner/system/input_multiplex_any_runner.hpp similarity index 100% rename from capabilities2_runner/include/capabilities2_runner/system/input_multiplex_any_runner.hpp rename to capabilities2_runner/docs/capabilities2_runner/system/input_multiplex_any_runner.hpp diff --git a/capabilities2_runner/include/capabilities2_runner/threadtrigger_runner.hpp b/capabilities2_runner/docs/capabilities2_runner/threadtrigger_runner.hpp similarity index 86% rename from capabilities2_runner/include/capabilities2_runner/threadtrigger_runner.hpp rename to capabilities2_runner/docs/capabilities2_runner/threadtrigger_runner.hpp index f56a78a..561ecf9 100644 --- a/capabilities2_runner/include/capabilities2_runner/threadtrigger_runner.hpp +++ b/capabilities2_runner/docs/capabilities2_runner/threadtrigger_runner.hpp @@ -7,6 +7,7 @@ #include #include +#include #include @@ -25,7 +26,8 @@ class ThreadTriggerRunner : public RunnerBase /** * @brief helper function to extract bond_id from thread_id * - * @param thread_id + * @param thread_id unique identifier for the execution thread, format: "bond_id/trigger_id" + * * @return const std::string */ static const std::string bond_from_thread_id(const std::string& thread_id) @@ -39,7 +41,7 @@ class ThreadTriggerRunner : public RunnerBase /** * @brief helper function to extract trigger_id from thread_id * - * @param thread_id + * @param thread_id unique identifier for the execution thread, format: "bond_id/trigger_id" * @return const std::string */ static const std::string trigger_from_thread_id(const std::string& thread_id) @@ -71,10 +73,11 @@ class ThreadTriggerRunner : public RunnerBase * @param bond_id unique identifier for the group of connections associated with this runner trigger event * */ - virtual void trigger(const std::string& parameters, const std::string& bond_id) override + virtual void trigger(const capabilities2::CapabilityOptions& parameters, const std::string& bond_id) override { // create a thread id std::string trigger_id = capabilities2_events::UUIDGenerator::gen_uuid_str(); + // namespace the thread id with bond id for later // could list all threads related to a bond if needed std::string thread_id = bond_id + "/" + trigger_id; @@ -84,7 +87,7 @@ class ThreadTriggerRunner : public RunnerBase std::scoped_lock lock(mutex_); // TODO: consider emitting on start event here // emit_event(bond_id, capabilities2_msgs::msg::CapabilityEventCode::ON_STARTED, updated_on_started(parameters)); - execution_thread_pool_[thread_id] = std::thread(&RunnerBase::execution, this, parameters, thread_id); + execution_thread_pool_[thread_id] = std::thread(&ThreadTriggerRunner::execution, this, parameters, thread_id); } // emit trigger event @@ -99,7 +102,6 @@ class ThreadTriggerRunner : public RunnerBase } protected: - // execution function to be implemented by child classes /** * @brief Trigger process to be executed. * @@ -108,9 +110,9 @@ class ThreadTriggerRunner : public RunnerBase * @param parameters parameters for the execution * @param thread_id unique identifier for the execution thread, can be used for tracking and cleanup * - * NOTE: should call success and failure events appropriately + * @attention: Should be implemented on derieved classes and should call success and failure events appropriately */ - virtual void execution(const std::string& parameters, const std::string& thread_id) = 0; + virtual void execution(const capabilities2::CapabilityOptions& parameters, const std::string& thread_id) = 0; private: /** */ diff --git a/capabilities2_runner/include/capabilities2_runner/topic_runner.hpp b/capabilities2_runner/docs/capabilities2_runner/topic_runner.hpp similarity index 97% rename from capabilities2_runner/include/capabilities2_runner/topic_runner.hpp rename to capabilities2_runner/docs/capabilities2_runner/topic_runner.hpp index a053ab9..ef2e66c 100644 --- a/capabilities2_runner/include/capabilities2_runner/topic_runner.hpp +++ b/capabilities2_runner/docs/capabilities2_runner/topic_runner.hpp @@ -74,7 +74,7 @@ class TopicRunner : public ThreadTriggerRunner * @param parameters pointer to tinyxml2::XMLElement that contains parameters * @param thread_id unique identifier for the execution thread */ - virtual void execution(const std::string& parameters, const std::string& thread_id) override + virtual void execution(const capabilities2::CapabilityOptions& parameters, const std::string& thread_id) override { // split thread_id to get bond_id and trigger_id (format: "bond_id/trigger_id") std::string bond_id = ThreadTriggerRunner::bond_from_thread_id(thread_id); diff --git a/capabilities2_runner/src/dummy_runner.cpp b/capabilities2_runner/src/dummy_runner.cpp index d7f3626..c06b636 100644 --- a/capabilities2_runner/src/dummy_runner.cpp +++ b/capabilities2_runner/src/dummy_runner.cpp @@ -38,7 +38,7 @@ class DummyRunner : public RunnerBase emit_stopped(bond_id, "dummy_parameters"); } - void trigger(const std::string& parameters, const std::string& bond_id) override + void trigger(const capabilities2::CapabilityOptions& parameters, const std::string& bond_id) override { RCLCPP_WARN(node_->get_logger(), "Dummy runner cannot trigger"); diff --git a/capabilities2_runner/src/get_capability_specs_runner.cpp b/capabilities2_runner/src/system/get_capability_specs_runner.cpp similarity index 100% rename from capabilities2_runner/src/get_capability_specs_runner.cpp rename to capabilities2_runner/src/system/get_capability_specs_runner.cpp diff --git a/capabilities2_runner/src/multiplex_all_runner.cpp b/capabilities2_runner/src/system/multiplex_all_runner.cpp similarity index 100% rename from capabilities2_runner/src/multiplex_all_runner.cpp rename to capabilities2_runner/src/system/multiplex_all_runner.cpp diff --git a/capabilities2_runner/src/multiplex_any_runner.cpp b/capabilities2_runner/src/system/multiplex_any_runner.cpp similarity index 100% rename from capabilities2_runner/src/multiplex_any_runner.cpp rename to capabilities2_runner/src/system/multiplex_any_runner.cpp diff --git a/capabilities2_server/include/capabilities2_server/capabilities_api.hpp b/capabilities2_server/include/capabilities2_server/capabilities_api.hpp index ccb913c..0af42a4 100644 --- a/capabilities2_server/include/capabilities2_server/capabilities_api.hpp +++ b/capabilities2_server/include/capabilities2_server/capabilities_api.hpp @@ -200,7 +200,7 @@ class CapabilitiesAPI * @param parameters * @param bond_id */ - void trigger_capability(const std::string& capability, const std::string& parameters, const std::string& bond_id) + void trigger_capability(const std::string& capability, const capabilities2::CapabilityOptions& parameters, const std::string& bond_id) { // validate bond if (!bond_cache_.exists(capability, bond_id)) diff --git a/capabilities2_server/include/capabilities2_server/capabilities_server.hpp b/capabilities2_server/include/capabilities2_server/capabilities_server.hpp index c751415..7ff3d88 100644 --- a/capabilities2_server/include/capabilities2_server/capabilities_server.hpp +++ b/capabilities2_server/include/capabilities2_server/capabilities_server.hpp @@ -11,7 +11,6 @@ #include #include -// #include #include diff --git a/capabilities2_server/include/capabilities2_server/runner_cache.hpp b/capabilities2_server/include/capabilities2_server/runner_cache.hpp index 8e1e686..cbdd656 100644 --- a/capabilities2_server/include/capabilities2_server/runner_cache.hpp +++ b/capabilities2_server/include/capabilities2_server/runner_cache.hpp @@ -82,7 +82,7 @@ class RunnerCache * @param parameters parameters related to the runner in std::string form for compatibility across various runners * @param bond_id unique identifier for the group on connections associated with this runner trigger */ - void trigger_runner(const std::string& capability, const std::string& parameters, const std::string& bond_id) + void trigger_runner(const std::string& capability, const capabilities2::CapabilityOptions& parameters, const std::string& bond_id) { // TODO: validate trigger id (DEPRECATED?) From e1f9ded9222fd55c21c0db2a58db29a3d8486919 Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Mon, 9 Feb 2026 15:57:19 +0000 Subject: [PATCH 25/37] wip --- .../capabilities2_events/event_node.hpp | 18 +- .../CapabilityOptions.hpp | 228 ++++---- .../capabilities2_runner/action_runner.hpp | 20 +- .../docs/capabilities2_runner/runner_base.hpp | 39 +- capabilities2_test_suite/.clang-format | 64 +++ capabilities2_test_suite/CMakeLists.txt | 111 ++++ .../action/Fibonacci.action | 5 + .../test_action_runner_node.hpp | 73 +++ .../test_runners/test_action_runner.hpp | 88 +++ .../test_systems/bond_client.hpp | 94 +++ .../test_systems/capabilities2_client.hpp | 543 ++++++++++++++++++ .../test_systems/test_action_server.hpp | 95 +++ .../interfaces/TestActionRunner.yaml | 17 + capabilities2_test_suite/package.xml | 36 ++ capabilities2_test_suite/plugins.xml | 7 + .../providers/TestActionRunner.yaml | 9 + .../src/test_action_runner_node.cpp | 28 + capabilities2_test_suite/src/test_runners.cpp | 5 + 18 files changed, 1324 insertions(+), 156 deletions(-) create mode 100644 capabilities2_test_suite/.clang-format create mode 100644 capabilities2_test_suite/CMakeLists.txt create mode 100644 capabilities2_test_suite/action/Fibonacci.action create mode 100644 capabilities2_test_suite/include/capabilities2_test_suite/test_action_runner_node.hpp create mode 100644 capabilities2_test_suite/include/capabilities2_test_suite/test_runners/test_action_runner.hpp create mode 100644 capabilities2_test_suite/include/capabilities2_test_suite/test_systems/bond_client.hpp create mode 100644 capabilities2_test_suite/include/capabilities2_test_suite/test_systems/capabilities2_client.hpp create mode 100644 capabilities2_test_suite/include/capabilities2_test_suite/test_systems/test_action_server.hpp create mode 100644 capabilities2_test_suite/interfaces/TestActionRunner.yaml create mode 100644 capabilities2_test_suite/package.xml create mode 100644 capabilities2_test_suite/plugins.xml create mode 100644 capabilities2_test_suite/providers/TestActionRunner.yaml create mode 100644 capabilities2_test_suite/src/test_action_runner_node.cpp create mode 100644 capabilities2_test_suite/src/test_runners.cpp diff --git a/capabilities2_events/include/capabilities2_events/event_node.hpp b/capabilities2_events/include/capabilities2_events/event_node.hpp index e39d550..3e2b306 100644 --- a/capabilities2_events/include/capabilities2_events/event_node.hpp +++ b/capabilities2_events/include/capabilities2_events/event_node.hpp @@ -83,10 +83,22 @@ class EventNode // get targets for this event type and id namespace if (connection.type.code == event_type && bond_id == conn_bond_id) { - // parameterise target capability - // create a copy of target with updated parameters + // parameterise target capability with parameters from the trigger capabilities2_msgs::msg::Capability target_with_params = connection.target; - target_with_params.parameters = parameters.toMsg().parameters; + + // extend or replace parameters of the target capability if any non empty parameters are provided + if (!parameters.is_empty()) + { + // convert parameters to msg format for easier merging + auto old_parameters = capabilities2::CapabilityOptions::fromMsg(target_with_params); + + // extend or replace parameters of the target capability + for (const auto& param : parameters) + old_parameters.set_value(param.key, param.type, param.value); + + // convert back to msg format + target_with_params = old_parameters.toMsg(); + } // emit event via event api // NOTE: callback invocation is handled by event api diff --git a/capabilities2_runner/docs/capabilities2_runner/CapabilityOptions.hpp b/capabilities2_runner/docs/capabilities2_runner/CapabilityOptions.hpp index 86cfb04..86fb0f0 100644 --- a/capabilities2_runner/docs/capabilities2_runner/CapabilityOptions.hpp +++ b/capabilities2_runner/docs/capabilities2_runner/CapabilityOptions.hpp @@ -4,7 +4,6 @@ #include #include #include -#include #include #include @@ -43,177 +42,184 @@ enum class OptionType * @param key the key of the option * @param value the value of the option */ -struct Options +struct Option { std::string key; std::vector value; OptionType type; - capabilities2_msgs::msg::CapabilityOption toMsg() const - { - capabilities2_msgs::msg::CapabilityOption msg; - msg.key = key; - msg.value = value; - msg.type = static_cast(type); - return msg; - } - - void fromMsg(const capabilities2_msgs::msg::CapabilityOption& msg) - { - key = msg.key; - value = msg.value; - type = static_cast(msg.type); - } -}; - -/** - * @brief capability options for a capability runner given by interface and provider - * - * @param interface the interface of the capability - * @param provider the provider of the capability - * @param options the options for the capability - */ -struct CapabilityOptions -{ - std::string interface; - std::string provider; - std::vector options; - - std::any get_value(const std::string& key) const - { - for (const auto& option : options) - if (option.key == key) - { - try - { - switch (option.type) + std::any get_value() + { + switch (type) { case OptionType::BOOL: - return option.value[0] == "true"; + return value[0] == "true"; case OptionType::DOUBLE: - return std::stod(option.value[0]); + return std::stod(value[0]); case OptionType::INT: - return std::stoi(option.value[0]); + return std::stoi(value[0]); case OptionType::STRING: - return option.value[0]; + return value[0]; case OptionType::VECTOR_BOOL: { std::vector vec; - for (const auto& v : option.value) + for (const auto& v : value) vec.push_back(v == "true"); return vec; } case OptionType::VECTOR_DOUBLE: { std::vector vec; - for (const auto& v : option.value) + for (const auto& v : value) vec.push_back(std::stod(v)); return vec; } case OptionType::VECTOR_INT: { std::vector vec; - for (const auto& v : option.value) + for (const auto& v : value) vec.push_back(std::stoi(v)); return vec; } case OptionType::VECTOR_STRING: - return option.value; + return value; + + } } - } - catch (const std::exception& e) - { - throw options_exception("Failed to convert option '" + option.key + "': " + e.what()); - } - - throw options_exception("Unknown option type for key: " + option.key); - } - } - void set_value(const std::string& key, const OptionType& type, const std::any& value) + void set_value(const OptionType& type, const std::any& value) { - for (auto& option : options) - if (option.key == key) - { switch (type) { case OptionType::BOOL: - option.value[0] = std::any_cast(value) ? "true" : "false"; + value[0] = std::any_cast(value) ? "true" : "false"; return; case OptionType::DOUBLE: - option.value[0] = std::to_string(std::any_cast(value)); + value[0] = std::to_string(std::any_cast(value)); return; case OptionType::INT: - option.value[0] = std::to_string(std::any_cast(value)); + value[0] = std::to_string(std::any_cast(value)); return; case OptionType::STRING: - option.value[0] = std::any_cast(value); + value[0] = std::any_cast(value); return; case OptionType::VECTOR_BOOL: { const auto& vec = std::any_cast>(value); - option.value.clear(); + value.clear(); for (const auto& v : vec) - option.value.push_back(v ? "true" : "false"); + value.push_back(v ? "true" : "false"); return; } case OptionType::VECTOR_DOUBLE: { const auto& vec = std::any_cast>(value); - option.value.clear(); + value.clear(); for (const auto& v : vec) - option.value.push_back(std::to_string(v)); + value.push_back(std::to_string(v)); return; } case OptionType::VECTOR_INT: { const auto& vec = std::any_cast>(value); - option.value.clear(); + value.clear(); for (const auto& v : vec) - option.value.push_back(std::to_string(v)); + value.push_back(std::to_string(v)); return; } case OptionType::VECTOR_STRING: - option.value = std::any_cast>(value); + value = std::any_cast>(value); return; } + } + + capabilities2_msgs::msg::CapabilityOption toMsg() const + { + capabilities2_msgs::msg::CapabilityOption msg; + msg.key = key; + msg.value = value; + msg.type = static_cast(type); + return msg; + } + + void fromMsg(const capabilities2_msgs::msg::CapabilityOption& msg) + { + key = msg.key; + value = msg.value; + type = static_cast(msg.type); + } +}; + +/** + * @brief capability options for a capability runner given by interface and provider + * + * @param interface the interface of the capability + * @param provider the provider of the capability + * @param options the options for the capability + */ +struct CapabilityOptions +{ + std::vector