diff --git a/.devcontainer/Dockerfile b/.devcontainer/Dockerfile index c3bad06..b93627b 100644 --- a/.devcontainer/Dockerfile +++ b/.devcontainer/Dockerfile @@ -27,4 +27,6 @@ RUN rosdep update RUN echo "source /opt/ros/${ROS_DISTRO}/setup.bash" >> ~/.bashrc # make sure folders exist -RUN mkdir -p ~/colcon_ws/src +WORKDIR /home/ubuntu/colcon_ws/src + +RUN git clone https://github.com/CollaborativeRoboticsLab/capabilities2_test_suite.git \ No newline at end of file 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/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/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/CMakeLists.txt b/capabilities2_events/CMakeLists.txt new file mode 100644 index 0000000..fb5ead3 --- /dev/null +++ b/capabilities2_events/CMakeLists.txt @@ -0,0 +1,54 @@ +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) + +# 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_package() 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..3c2340e --- /dev/null +++ b/capabilities2_events/include/capabilities2_events/event_base.hpp @@ -0,0 +1,132 @@ +#pragma once + +#include +#include +#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: + typedef std::string capability_str_t; + typedef capabilities2_events::EventParameters parameter_t; + typedef std::string access_id_t; + typedef std::string target_instance_id_t; + typedef std::function event_callback_t; + +public: + EventBase() + { + } + + ~EventBase() = default; + + /** + * @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 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, target_instance_id) + */ + 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) + { + // extract bond_id from connection_id (format: "bond_id/instance_id/target_instance_id") + size_t first_pos = connection_id.find('/'); + + std::string bond_id = (first_pos != std::string::npos) ? connection_id.substr(0, first_pos) : connection_id; + + // do callback with bond_id for access control + if (callback) + { + callback(bond_id, target.capability, target.instance_id, capabilities2_events::EventParameters(target)); + } + } + + /** + * @brief on server ready event + * + * @param msg + */ + virtual void on_server_ready(const std::string& msg) = 0; + + /** + * @brief on process launched event + * + * @param pid + */ + virtual void on_process_launched(const std::string& pid) = 0; + + /** + * @brief on process terminated event + * + * @param pid + */ + virtual void on_process_terminated(const std::string& pid) = 0; + + /** + * @brief on triggered event from server + * + * @param trigger_id + */ + virtual void on_triggered(const std::string& trigger_id) = 0; + + /** + * @brief on connected event from server + * + * @param source + * @param target + */ + virtual void on_connected(const std::string& source, const std::string& target) = 0; + + /** + * @brief on disconnected event from server + * + * @param source + * @param target + */ + virtual void on_disconnected(const std::string& source, const std::string& target) = 0; +}; + +} // 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..c18226e --- /dev/null +++ b/capabilities2_events/include/capabilities2_events/event_node.hpp @@ -0,0 +1,300 @@ +#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 with signature: (capability, parameters, bond_id) + EventBase::event_callback_t callback; + }; + +public: + EventNode(std::shared_ptr event_emitter = nullptr) + : id_(UUIDGenerator::gen_uuid_str()), 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 the bond_id to match connections with (extracted from connection_id) for access control + * @param event_type the type of event being emitted + * @param msg_parameters the new parameters to emit with the event + */ + void emit_event(const std::string& bond_id, const std::string& instance_id, const uint8_t& event_type, + capabilities2_events::EventParameters parameters = capabilities2_events::EventParameters()) + { + // 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/instance_id/target_instance_id") + size_t first_pos = conn_id.find('/'); + size_t second_pos = conn_id.find('/', first_pos + 1); + + std::string conn_bond_id = (first_pos != std::string::npos) ? conn_id.substr(0, first_pos) : conn_id; + std::string conn_instance_id = + (second_pos != std::string::npos) ? conn_id.substr(first_pos + 1, second_pos - first_pos - 1) : ""; + + // get targets for this event type and id namespace + if (connection.type.code == event_type && bond_id == conn_bond_id && instance_id == conn_instance_id) + { + // parameterise target capability with parameters from the trigger + auto old_parameters = capabilities2_events::EventParameters(connection.target); + + // extend or replace parameters of the target capability if any non empty parameters are provided + if (!parameters.is_empty()) + for (auto& option : parameters.options) + old_parameters.set_value(option.key, option.get_value(), option.type); + + // create a new target capability message with updated parameters to emit with the event + auto target_with_params = old_parameters.toMsg(); + + // copy capability and provider from original target as parameters only contains the options + target_with_params.capability = connection.target.capability; + target_with_params.provider = connection.target.provider; + target_with_params.instance_id = connection.target.instance_id; + + // 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 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 event_cb callback to trigger target capability with (capability, parameters, bond_id, target_instance_id) + * + * @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, EventBase::event_callback_t 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; + + if (event_emitter_) + { + // emit connected event + event_emitter_->on_connected(source_.capability, target.capability); + } + } + + /** + * @brief Remove a connection by its id + * + * @param connection_id + */ + void remove_connection(const std::string& connection_id) + { + // remove connection + auto it = connections_.find(connection_id); + if (it == connections_.end()) + { + throw event_exception("connection with id: " + connection_id + " does not exist"); + } + auto target = it->second.target; + connections_.erase(connection_id); + + // emit disconnected event + if (event_emitter_) + { + event_emitter_->on_disconnected(source_.capability, target.capability); + } + } + + // 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_; + } + + /** + * @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 + 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/event_parameters.hpp b/capabilities2_events/include/capabilities2_events/event_parameters.hpp new file mode 100644 index 0000000..f99d60b --- /dev/null +++ b/capabilities2_events/include/capabilities2_events/event_parameters.hpp @@ -0,0 +1,268 @@ +#pragma once + +#include +#include +#include +#include +#include +#include + +namespace capabilities2_events +{ + +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, + UNCONVERTED, + 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 Parameter +{ + std::string key; + std::vector value; + OptionType type; + + Parameter() = default; + + Parameter(const capabilities2_msgs::msg::CapabilityParameter& msg) + { + key = msg.key; + value = msg.value; + type = static_cast(msg.type); + } + + std::any get_value() + { + switch (type) + { + case OptionType::BOOL: + return value[0] == "true"; + case OptionType::DOUBLE: + return std::stod(value[0]); + case OptionType::INT: + return std::stoi(value[0]); + case OptionType::STRING: + return value[0]; + case OptionType::VECTOR_BOOL: { + std::vector vec; + for (const auto& v : value) + vec.push_back(v == "true"); + return vec; + } + case OptionType::VECTOR_DOUBLE: { + std::vector vec; + for (const auto& v : value) + vec.push_back(std::stod(v)); + return vec; + } + case OptionType::VECTOR_INT: { + std::vector vec; + for (const auto& v : value) + vec.push_back(std::stoi(v)); + return vec; + } + case OptionType::VECTOR_STRING: + return value; + } + } + + void set_value(std::string new_key, std::any new_value, OptionType new_type) + { + key = new_key; + type = new_type; + + switch (type) + { + case OptionType::BOOL: + value.clear(); + value.push_back(std::any_cast(new_value) ? "true" : "false"); + return; + case OptionType::DOUBLE: + value.clear(); + value.push_back(std::to_string(std::any_cast(new_value))); + return; + case OptionType::INT: + value.clear(); + value.push_back(std::to_string(std::any_cast(new_value))); + return; + case OptionType::STRING: + value.clear(); + value.push_back(std::any_cast(new_value)); + return; + case OptionType::VECTOR_BOOL: { + const auto& vec = std::any_cast>(new_value); + value.clear(); + for (const auto& v : vec) + value.push_back(v ? "true" : "false"); + return; + } + case OptionType::VECTOR_DOUBLE: { + const auto& vec = std::any_cast>(new_value); + value.clear(); + for (const auto& v : vec) + value.push_back(std::to_string(v)); + return; + } + case OptionType::VECTOR_INT: { + const auto& vec = std::any_cast>(new_value); + value.clear(); + for (const auto& v : vec) + value.push_back(std::to_string(v)); + return; + } + case OptionType::VECTOR_STRING: + value = std::any_cast>(new_value); + return; + + default: + throw options_exception("Unsupported OptionType"); + } + } + + capabilities2_msgs::msg::CapabilityParameter toMsg() const + { + capabilities2_msgs::msg::CapabilityParameter msg; + msg.key = key; + msg.value = value; + msg.type = static_cast(type); + return msg; + } +}; + +/** + * @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 EventParameters +{ + std::vector options = {}; + + EventParameters() = default; + + EventParameters(const capabilities2_msgs::msg::Capability& msg) + { + options.clear(); + for (const auto& option_msg : msg.parameters) + { + auto option = Parameter(option_msg); + options.push_back(option); + } + } + + bool is_empty() const + { + return options.empty(); + } + + /** + * @brief Check if an option with the given key exists + * + * @param key the key of the option to check + * @return true if the option exists, false otherwise + */ + bool has_value(const std::string& key) const + { + for (const auto& option : options) + if (option.key == key) + return true; + return false; + } + + /** + * @brief Get the value of an option by key + * + * @param key the key of the option to get the value of + * @return std::any the value of the option, can be cast to the appropriate type based on the OptionType + * @throws options_exception if the key is not found or if there is a type conversion error + */ + std::any get_value(const std::string& key, std::any default_value) + { + if (has_value(key)) + for (auto& option : options) + { + if (option.key == key) + { + try + { + return option.get_value(); + } + catch (const std::exception& e) + { + throw options_exception("Failed to convert option '" + option.key + "': " + e.what()); + } + } + } + else + return default_value; + } + + /** + * @brief Set the value of an option by key, if the option does not exist it will be created + * + * @param key the key of the option to set the value of + * @param type the type of the option to set + * @param value the value to set, should be castable to the appropriate type based on the OptionType + * @throws options_exception if there is a type conversion error + */ + void set_value(const std::string& key, const std::any& value, const OptionType& type) + { + for (auto& option : options) + if (option.key == key) + { + try + { + option.set_value(key, value, type); + return; + } + catch (const std::exception& e) + { + throw options_exception("Failed to set option '" + option.key + "': " + e.what()); + } + } + + // Parameter not found, add new one + Parameter new_option; + new_option.set_value(key, value, type); + options.push_back(new_option); + } + + capabilities2_msgs::msg::Capability toMsg() const + { + capabilities2_msgs::msg::Capability msg; + for (const auto& option : options) + msg.parameters.push_back(option.toMsg()); + return msg; + } +}; + +} // namespace capabilities2_events \ No newline at end of file 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..436c864 --- /dev/null +++ b/capabilities2_events/include/capabilities2_events/published_event.hpp @@ -0,0 +1,140 @@ +#pragma once + +#include + +#include +#include + +namespace capabilities2_events +{ + +class PublishedEvent : public EventBase +{ +public: + PublishedEvent(rclcpp::Publisher::SharedPtr event_pub) + : EventBase(), event_pub_(event_pub) + { + } + + ~PublishedEvent() = default; + + /** + * @brief see EventBase::emit, specialised to publish events + */ + 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 + { + // split connection id to get trigger id. Assuming connection_id format is "bond_id/instance_id/target_instance_id" + size_t first_pos = connection_id.find('/'); + size_t second_pos = connection_id.find('/', first_pos + 1); + + std::string instance_id = (second_pos != std::string::npos) ? connection_id.substr(first_pos + 1, second_pos - first_pos - 1) : ""; + + capabilities2_msgs::msg::CapabilityEventStamped event_msg; + event_msg.header.stamp = rclcpp::Clock().now(); + event_msg.event.trigger_id = instance_id; + event_msg.event.code.code = event_code; + event_msg.event.connection.source = source; + event_msg.event.connection.target = target; + + // publish event + event_pub_->publish(event_msg); + + // call super + 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.connection.source.capability = "capabilities2_server"; + event_msg.event.connection.source.provider = "capabilities2_server"; + event_msg.event.code.code = capabilities2_msgs::msg::CapabilityEventCode::SERVER_READY; + event_msg.event.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.connection.source.capability = "capabilities2_server"; + event_msg.event.connection.source.provider = "capabilities2_server"; + event_msg.event.code.code = capabilities2_msgs::msg::CapabilityEventCode::LAUNCHED; + event_msg.event.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.connection.source.capability = "capabilities2_server"; + event_msg.event.connection.source.provider = "capabilities2_server"; + event_msg.event.code.code = capabilities2_msgs::msg::CapabilityEventCode::TERMINATED; + event_msg.event.description = "Process terminated with PID: " + pid; + + event_pub_->publish(event_msg); + } + + void on_triggered(const std::string& trigger_id) + { + capabilities2_msgs::msg::CapabilityEventStamped event_msg; + event_msg.header.stamp = rclcpp::Clock().now(); + event_msg.event.connection.source.capability = "capabilities2_server"; + event_msg.event.connection.source.provider = "capabilities2_server"; + event_msg.event.code.code = capabilities2_msgs::msg::CapabilityEventCode::TRIGGERED; + event_msg.event.description = "Triggered event with ID: " + trigger_id; + + event_pub_->publish(event_msg); + } + + void on_connected(const std::string& source, const std::string& target) + { + capabilities2_msgs::msg::CapabilityEventStamped event_msg; + event_msg.header.stamp = rclcpp::Clock().now(); + event_msg.event.connection.source.capability = "capabilities2_server"; + event_msg.event.connection.source.provider = "capabilities2_server"; + event_msg.event.code.code = capabilities2_msgs::msg::CapabilityEventCode::CONNECTED; + event_msg.event.description = "Connected event from " + source + " to " + target; + + event_pub_->publish(event_msg); + } + + void on_disconnected(const std::string& source, const std::string& target) + { + capabilities2_msgs::msg::CapabilityEventStamped event_msg; + event_msg.header.stamp = rclcpp::Clock().now(); + event_msg.event.connection.source.capability = "capabilities2_server"; + event_msg.event.connection.source.provider = "capabilities2_server"; + event_msg.event.code.code = capabilities2_msgs::msg::CapabilityEventCode::DISCONNECTED; + event_msg.event.description = "Disconnected event from " + source + " to " + target; + + event_pub_->publish(event_msg); + } + +private: + // event publisher + rclcpp::Publisher::SharedPtr event_pub_; +}; + +} // namespace capabilities2_events 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 new file mode 100644 index 0000000..e26ef65 --- /dev/null +++ b/capabilities2_events/package.xml @@ -0,0 +1,26 @@ + + + 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 + uuid + + + ament_cmake + + diff --git a/capabilities2_events/readme.md b/capabilities2_events/readme.md new file mode 100644 index 0000000..6b85b7e --- /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 +- [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 +- [x] 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_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 diff --git a/capabilities2_msgs/CMakeLists.txt b/capabilities2_msgs/CMakeLists.txt index af7c253..3be2079 100644 --- a/capabilities2_msgs/CMakeLists.txt +++ b/capabilities2_msgs/CMakeLists.txt @@ -20,31 +20,33 @@ set(msg_files "msg/CapabilityCommand.msg" "msg/CapabilityConnection.msg" "msg/CapabilityEvent.msg" + "msg/CapabilityEventCode.msg" "msg/CapabilityEventStamped.msg" "msg/CapabilityResponse.msg" "msg/CapabilitySpec.msg" "msg/NaturalCapability.msg" "msg/Remapping.msg" "msg/RunningCapability.msg" + "msg/CapabilityParameter.msg" ) 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/Capability.msg b/capabilities2_msgs/msg/Capability.msg index 5bf97a1..8d08754 100644 --- a/capabilities2_msgs/msg/Capability.msg +++ b/capabilities2_msgs/msg/Capability.msg @@ -4,5 +4,8 @@ string capability # Used provider string provider -# trigger parameters -string parameters +# Trigger parameters +CapabilityParameter[] parameters + +# What instance this is being referred +string instance_id \ No newline at end of file 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 8862383..9501444 100644 --- a/capabilities2_msgs/msg/CapabilityEvent.msg +++ b/capabilities2_msgs/msg/CapabilityEvent.msg @@ -1,46 +1,16 @@ -# the capability event message type +# the capability event message -# Capabilities which this event pertains to (source and target) -# also represents connections between capabilities +# identifying name for the source that triggered this event +string trigger_id + +# 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 -# 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..e8fb533 --- /dev/null +++ b/capabilities2_msgs/msg/CapabilityEventCode.msg @@ -0,0 +1,25 @@ +# available 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 +# runner configuration events +uint8 CONNECTED=5 +uint8 DISCONNECTED=6 +uint8 TRIGGERED=7 +# system events +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 +uint8 UNDEFINED=19 + +# highest code +uint8 MAX_CODE=20 + +# the event code +uint8 code diff --git a/capabilities2_msgs/msg/CapabilityParameter.msg b/capabilities2_msgs/msg/CapabilityParameter.msg new file mode 100644 index 0000000..e14ef44 --- /dev/null +++ b/capabilities2_msgs/msg/CapabilityParameter.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_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..aad4b7e --- /dev/null +++ b/capabilities2_msgs/srv/ConnectCapability.srv @@ -0,0 +1,5 @@ +string bond_id # user + +# connect using a connection message +capabilities2_msgs/CapabilityConnection connection +--- diff --git a/capabilities2_msgs/srv/TriggerCapability.srv b/capabilities2_msgs/srv/TriggerCapability.srv index b9160a7..0a52ba9 100644 --- a/capabilities2_msgs/srv/TriggerCapability.srv +++ b/capabilities2_msgs/srv/TriggerCapability.srv @@ -1,3 +1,3 @@ -string capability -string parameters +string bond_id +capabilities2_msgs/Capability capability --- diff --git a/capabilities2_runner/CMakeLists.txt b/capabilities2_runner/CMakeLists.txt index 6021b30..4e221d9 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,18 +15,16 @@ 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(tinyxml2_vendor REQUIRED) -find_package(TinyXML2 REQUIRED) # provided by tinyxml2 upstream, or tinyxml2_vendor +find_package(capabilities2_events REQUIRED) include_directories( include ) add_library(${PROJECT_NAME} SHARED - src/standard_runners.cpp + src/dummy_runner.cpp + src/launch_runner.cpp + src/system/system_runners.cpp ) ament_target_dependencies(${PROJECT_NAME} @@ -34,10 +32,7 @@ ament_target_dependencies(${PROJECT_NAME} rclcpp_action pluginlib capabilities2_msgs - capabilities2_utils - event_logger - event_logger_msgs - TinyXML2 + capabilities2_events ) pluginlib_export_plugin_description_file(${PROJECT_NAME} plugins.xml) @@ -53,6 +48,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/include/capabilities2_runner/action_runner.hpp b/capabilities2_runner/include/capabilities2_runner/action_runner.hpp index 7c768df..61f7b8c 100644 --- a/capabilities2_runner/include/capabilities2_runner/action_runner.hpp +++ b/capabilities2_runner/include/capabilities2_runner/action_runner.hpp @@ -1,17 +1,19 @@ #pragma once #include -#include #include #include #include +#include +#include +#include +#include -#include #include #include #include -#include +#include namespace capabilities2_runner { @@ -22,13 +24,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 +50,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: %s", action_name.c_str()); 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: %s", action_name.c_str()); throw runner_exception("failed to connect to action server"); } - info_("connected with action: " + action_name); + RCLCPP_INFO(node_->get_logger(), "connected with action: %s", action_name.c_str()); } /** * @brief stop function to cease functionality and shutdown * */ - virtual void stop() override + virtual void stop(const std::string& bond_id, const std::string& instance_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 @@ -83,21 +85,15 @@ class ActionRunner : public RunnerBase try { auto cancel_future = action_client_->async_cancel_goal( - goal_handle_, [this](action_msgs::srv::CancelGoal_Response::SharedPtr response) { + goal_handle_, [this, &bond_id, &instance_id](action_msgs::srv::CancelGoal_Response::SharedPtr response) { 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, instance_id, param_on_stopped()); }); // wait for action to be stopped. hold the thread for 2 seconds to help keep callbacks in scope @@ -112,116 +108,95 @@ 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: %s", 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. * * This method utilizes paramters set via the trigger() function * - * @param parameters pointer to tinyxml2::XMLElement that contains parameters + * @param parameters pointer to capabilities2_events::EventParameters that contains parameters */ - virtual void execution(int id) override + virtual void execution(capabilities2_events::EventParameters parameters, const std::string& thread_id) override { - // if parameters are not provided then cannot proceed - if (!parameters_[id]) - throw runner_exception("cannot trigger action without parameters"); - - // generate a goal from parameters if provided - goal_msg_ = generate_goal(parameters_[id], id); + // split thread_id to get bond_id and instance_id (format: "bond_id/instance_id") + std::string bond_id = ThreadTriggerRunner::bond_from_thread_id(thread_id); + std::string instance_id = ThreadTriggerRunner::instance_from_thread_id(thread_id); - info_("goal generated for event ", id); + // generate a goal from parameters provided + goal_msg_ = generate_goal(parameters); + RCLCPP_INFO(node_->get_logger(), "goal generated for instance %s", instance_id.c_str()); - std::unique_lock lock(mutex_); - completed_ = false; + std::mutex block_mutex; + std::condition_variable cv; + bool completed = false; + std::unique_lock lock(block_mutex); // trigger the action client with goal send_goal_options_.goal_response_callback = - [this, id](const typename rclcpp_action::ClientGoalHandle::SharedPtr& goal_handle) { + [this, &instance_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 for instance %s", instance_id.c_str()); } else { - error_("goal rejected", id); + RCLCPP_ERROR(node_->get_logger(), "goal rejected for instance %s", instance_id.c_str()); } // store goal handle to be used with stop funtion goal_handle_ = goal_handle; }; - send_goal_options_.feedback_callback = - [this, id](typename rclcpp_action::ClientGoalHandle::SharedPtr goal_handle, - const typename ActionT::Feedback::ConstSharedPtr feedback_msg) { - std::string feedback = generate_feedback(feedback_msg); + send_goal_options_.feedback_callback = [this, &instance_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); - } - }; + if (feedback != "") + { + RCLCPP_INFO(node_->get_logger(), "received feedback: %s for instance %s", feedback.c_str(), instance_id.c_str()); + } + }; send_goal_options_.result_callback = - [this, id](const typename rclcpp_action::ClientGoalHandle::WrappedResult& wrapped_result) { - info_("received result", id); + [this, &instance_id, &completed, &cv, + &bond_id, &instance_id](const typename rclcpp_action::ClientGoalHandle::WrappedResult& wrapped_result) { + RCLCPP_INFO(node_->get_logger(), "received result for instance %s", instance_id.c_str()); 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 instance %s", instance_id.c_str()); + // emit success event + emit_succeeded(bond_id, instance_id, param_on_success()); } 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 instance %s", instance_id.c_str()); + + // emit failed event + emit_failed(bond_id, instance_id, param_on_failure()); } result_ = wrapped_result.result; - completed_ = true; - cv_.notify_all(); + completed = true; + cv.notify_all(); }; 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 instance %s", instance_id.c_str()); // Conditional wait - cv_.wait(lock, [this] { return completed_; }); - info_("action complete. Thread closing.", id); + cv.wait(lock, [&completed] { return completed; }); + RCLCPP_INFO(node_->get_logger(), "action complete. Thread closing for instance %s", instance_id.c_str()); } -protected: /** * @brief Generate a goal from parameters * @@ -230,10 +205,10 @@ class ActionRunner : public RunnerBase * * A pattern needs to be implemented in the derived class * - * @param parameters + * @param parameters capability options that contain parameters for the instance * @return ActionT::Goal the generated goal */ - virtual typename ActionT::Goal generate_goal(tinyxml2::XMLElement* parameters, int id) = 0; + virtual typename ActionT::Goal generate_goal(capabilities2_events::EventParameters& parameters) = 0; /** * @brief Generate a std::string from feedback message @@ -243,11 +218,12 @@ class ActionRunner : public RunnerBase * 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 * @return ActionT::Feedback the received feedback */ virtual std::string generate_feedback(const typename ActionT::Feedback::ConstSharedPtr msg) = 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..4f059a6 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, const std::string& instance_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, instance_id); + + // emit stopped event + emit_stopped(bond_id, instance_id, param_on_stopped()); } // 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 b4c101d..68a8166 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, "", param_on_started()); } /** * @brief stop function to cease functionality and shutdown * */ - virtual void stop() override + virtual void stop(const std::string& bond_id, const std::string& instance_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, instance_id, param_on_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 \ No newline at end of file +} // 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..d124089 --- /dev/null +++ b/capabilities2_runner/include/capabilities2_runner/notrigger_runner.hpp @@ -0,0 +1,29 @@ +#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(capabilities2_events::EventParameters& parameters, const std::string& bond_id, + const std::string& instance_id = "") override + { + // emit failed event + emit_failed(bond_id, instance_id, param_on_failure()); + + 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 b0e20ae..2aefd08 100644 --- a/capabilities2_runner/include/capabilities2_runner/runner_base.hpp +++ b/capabilities2_runner/include/capabilities2_runner/runner_base.hpp @@ -3,16 +3,14 @@ #include #include #include -#include -#include -#include + #include -#include +#include +#include -#include -#include -#include +#include +#include namespace capabilities2_runner { @@ -66,13 +64,16 @@ 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; - - RunnerBase() : run_config_() + RunnerBase() : node_(nullptr), run_config_() { } @@ -87,40 +88,34 @@ 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_ + * + * @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) = 0; + virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config, const std::string& bond_id) = 0; /** * @brief stop the runner * + * @attention should clean up threads and should call stop event */ - virtual void stop() = 0; + virtual void stop(const std::string& bond_id, const std::string& instance_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 + * @param instance_id unique identifier for the instance of the capability + * @attention should call success and failure events with parameters and bond_id when the trigger process + * completes. * */ - virtual void trigger(const std::string& parameters) - { - // 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); - - parameters_[runner_id] = element; - - info_("received new parameters with event id : " + std::to_string(runner_id), runner_id); - - executionThreadPool[runner_id] = std::thread(&RunnerBase::execution, this, runner_id); - - info_("started execution", runner_id); - } + virtual void trigger(capabilities2_events::EventParameters& parameters, const std::string& bond_id, + const std::string& instance_id) = 0; /** * @brief Initializer function for initializing the base runner in place of constructor due to plugin semantics @@ -130,32 +125,49 @@ 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; - - 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); + } } /** - * @brief attach events to the runner + * @brief make EventNode::add_connection public method on runner api. * - * @param event_option event_options related for the action - * @param triggerFunction external function that triggers capability runners + * 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 * - * @return number of attached events + * @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) */ - virtual void attach_events(capabilities2::event_opts& event_option, - std::function triggerFunction) + void add_connection(const std::string& connection_id, const capabilities2_msgs::msg::CapabilityEventCode& type, + const capabilities2_msgs::msg::Capability& target, + std::function + callback) { - info_("accepted event options with ID : " + std::to_string(event_option.event_id)); - - triggerFunction_ = triggerFunction; - - events[event_option.event_id] = event_option; + EventNode::add_connection(connection_id, type, target, callback); } /** @@ -198,136 +210,69 @@ class RunnerBase return run_config_.pid; } - /** - * @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 paramters set via the trigger() function - * - * @param id unique identifier for the runner id. used to track the correct - * triggers and subsequent events. - * - */ - virtual void execution(int id) = 0; + // FIXME: implement new event subsystem /** - * @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 + * 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 + * @return CapabilityOptions containing new parameters */ - virtual std::string update_on_started(std::string& parameters) + virtual capabilities2_events::EventParameters param_on_started() { - return parameters; + return capabilities2_events::EventParameters(); }; /** - * @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 + * 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 + * @return CapabilityOptions containing new parameters */ - virtual std::string update_on_stopped(std::string& parameters) + virtual capabilities2_events::EventParameters param_on_stopped() { - return parameters; + return capabilities2_events::EventParameters(); }; /** - * @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 + * 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 + * @return CapabilityOptions containing new parameters */ - virtual std::string update_on_failure(std::string& parameters) + virtual capabilities2_events::EventParameters param_on_failure() { - return parameters; + return capabilities2_events::EventParameters(); }; /** - * @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 + * 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 + * @return CapabilityOptions containing new parameters */ - virtual std::string update_on_success(std::string& parameters) + virtual capabilities2_events::EventParameters param_on_success() { - return parameters; + return capabilities2_events::EventParameters(); }; - /** - * @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 /** @@ -461,106 +406,65 @@ class RunnerBase return get_first_resource_name("action"); } -protected: - void info_(const std::string text, int thread_id = -1) + // FIXME: implement new event subsystem + // STATE CHANGE HELPERS + + /** + * @brief emit STARTED event + * + * @param bond_id + * @param parameters + */ + void emit_started(const std::string& bond_id, const std::string& instance_id, + capabilities2_events::EventParameters parameters = capabilities2_events::EventParameters()) { - 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); + RCLCPP_INFO(node_->get_logger(), "emitting STARTED event with bond_id: %s", bond_id.c_str()); + emit_event(bond_id, instance_id, capabilities2_msgs::msg::CapabilityEventCode::STARTED, parameters); } - void error_(const std::string text, int thread_id = -1) + /** + * @brief emit STOPPED event + * + * @param bond_id + * @param parameters + */ + void emit_stopped(const std::string& bond_id, const std::string& instance_id, + capabilities2_events::EventParameters parameters = capabilities2_events::EventParameters()) { - 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); + RCLCPP_INFO(node_->get_logger(), "emitting STOPPED event with bond_id: %s", bond_id.c_str()); + emit_event(bond_id, instance_id, capabilities2_msgs::msg::CapabilityEventCode::STOPPED, parameters); } - void output_(const std::string text, const std::string element, int thread_id = -1) + /** + * @brief emit SUCCEEDED event + * + * @param bond_id + * @param parameters + */ + void emit_succeeded(const std::string& bond_id, const std::string& instance_id, + capabilities2_events::EventParameters parameters = capabilities2_events::EventParameters()) { - 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); + RCLCPP_INFO(node_->get_logger(), "emitting SUCCEEDED event with bond_id: %s", bond_id.c_str()); + emit_event(bond_id, instance_id, capabilities2_msgs::msg::CapabilityEventCode::SUCCEEDED, parameters); } - void event_(EventType event = EventType::IDLE, int thread_id = -1, const std::string& target_capability = "", - const std::string& target_provider = "") + /** + * @brief emit FAILED event + * + * @param bond_id + * @param parameters + */ + void emit_failed(const std::string& bond_id, const std::string& instance_id, + capabilities2_events::EventParameters parameters = capabilities2_events::EventParameters()) { - 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; - } - - event_client_->publish(message); + RCLCPP_INFO(node_->get_logger(), "emitting FAILED event with bond_id: %s", bond_id.c_str()); + emit_event(bond_id, instance_id, capabilities2_msgs::msg::CapabilityEventCode::FAILED, parameters); } +protected: /** - * @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_; @@ -568,71 +472,6 @@ class RunnerBase * @brief run_config_ runner configuration */ runner_opts run_config_; - - /** - * @brief dictionary of events - */ - std::map events; - - /** - * @brief unique id for the runner - */ - int runner_id; - - /** - * @brief curent number of trigger signals received - */ - int current_inputs_; - - /** - * @brief system runner completion tracking - */ - bool execution_complete_; - - /** - * @brief pointer to XMLElement which contain parameters - */ - std::map parameters_; - - /** - * @brief dictionary of threads that executes the triggerExecution functionality - */ - std::map executionThreadPool; - - /** - * @brief mutex for threadpool synchronisation. - */ - std::mutex mutex_; - - /** - * @brief conditional variable for threadpool synchronisation. - */ - std::condition_variable cv_; - - /** - * @brief flag for threadpool synchronisation. - */ - bool completed_; - - /** - * @brief external function that triggers capability runners - */ - std::function triggerFunction_; - - /** - * @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 for publishing events - */ - std::shared_ptr event_client_; }; } // 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..2a9e9c9 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,97 @@ 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: %s", service_name.c_str()); 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: %s", service_name.c_str()); throw runner_exception("failed to connect to server"); } - info_("connected with service: " + service_name); + RCLCPP_INFO(node_->get_logger(), "connected with service: %s", service_name.c_str()); } + /** + * @brief stop function to cease functionality and shutdown + * + */ + virtual void stop(const std::string& bond_id, const std::string& instance_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, instance_id, param_on_stopped()); + + 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(capabilities2_events::EventParameters parameters, const std::string& thread_id) override { - // if parameters are not provided then cannot proceed - if (!parameters_[id]) - throw runner_exception("cannot trigger service without parameters"); + // split thread_id to get bond_id and instance_id (format: "bond_id/instance_id") + std::string bond_id = ThreadTriggerRunner::bond_from_thread_id(thread_id); + std::string instance_id = ThreadTriggerRunner::instance_from_thread_id(thread_id); // 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)); - info_("request generated for event :", id); + RCLCPP_INFO(node_->get_logger(), "request generated for event :%s", instance_id.c_str()); - std::unique_lock lock(mutex_); - completed_ = false; + std::mutex block_mutex; + std::unique_lock lock(block_mutex); + std::condition_variable cv; + bool completed = false; auto result_future = service_client_->async_send_request( - request_msg, [this, id](typename rclcpp::Client::SharedFuture future) { + request_msg, [this, &instance_id, &completed, &bond_id, &cv](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, instance_id, param_on_failure()); } else { - info_("get result call succeeded", id); + RCLCPP_INFO(node_->get_logger(), "get result call succeeded for event :%s", instance_id.c_str()); 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_); + + // emit success event + emit_succeeded(bond_id, instance_id, param_on_success()); } - completed_ = true; - cv_.notify_all(); + 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.."); + cv.wait(lock, [&completed] { return completed; }); + RCLCPP_INFO(node_->get_logger(), "Service request complete. Thread closing."); } protected: @@ -162,22 +138,24 @@ class ServiceRunner : public RunnerBase * * A pattern needs to be implemented in the derived class * - * @param parameters + * @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(capabilities2_events::EventParameters& parameters) = 0; /** * @brief Process the reponse and print data as required * - * @param response service reponse - * @param 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_events::EventParameters 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, int id) - { - } + virtual void process_response(typename ServiceT::Response::SharedPtr /*response*/) {} 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/system/get_capability_specs_runner.hpp b/capabilities2_runner/include/capabilities2_runner/system/get_capability_specs_runner.hpp new file mode 100644 index 0000000..29557d3 --- /dev/null +++ b/capabilities2_runner/include/capabilities2_runner/system/get_capability_specs_runner.hpp @@ -0,0 +1,84 @@ +#pragma once + +#include +#include + +namespace capabilities2_runner +{ + +/** + * @brief Executor runner class + * + * Class to run capabilities2 executor action based capability + * + */ +class GetCapabilitySpecsRunner : public ServiceRunner +{ +public: + GetCapabilitySpecsRunner() : ServiceRunner() + { + } + + /** + * @brief Starter function for starting the action runner + * + * @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 unique identifier for the group of connections associated with this runner trigger event + */ + virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config, const std::string& bond_id) override + { + init_service(node, run_config, "/capabilities/get_capability_specs"); + + // emit start event + emit_started(bond_id, "", param_on_started()); + } + +protected: + /** + * @brief This generate goal function overrides the generate_goal() function from ActionRunner() + * @param parameters XMLElement that contains parameters in the format + '' + * @return ActionT::Goal the generated goal + */ + virtual capabilities2_msgs::srv::GetCapabilitySpecs::Request + generate_request(capabilities2_events::EventParameters& parameters) override + { + capabilities2_msgs::srv::GetCapabilitySpecs::Request request; + return request; + } + + /** + * @brief This function overrides the param_on_success() function from RunnerBase to provide specific implementation for the GetCapabilitySpecsRunner + * + * @param parameters EventParameters containing parameters for the trigger event + * @return EventParameters updated parameters for on_success event + */ + virtual capabilities2_events::EventParameters param_on_success() override + { + // Create an Event Parameter with CapabilitySpecs content + std::vector capability_spec_strings; + for (const auto& spec : response_->capability_specs) + { + std::string spec_string = "package: " + spec.package + ", type: " + spec.type + + ", default_provider: " + spec.default_provider + ", content: " + spec.content; + capability_spec_strings.push_back(spec_string); + } + + // Create EventParameters and set the capability specs as a parameter for the on_success event + capabilities2_events::EventParameters parameters; + parameters.set_value("CapabilitySpecs", capability_spec_strings, capabilities2_events::OptionType::VECTOR_STRING); + + RCLCPP_INFO(node_->get_logger(), "GetCapabilitySpecsRunner creating param_on_success with %zu capability specs", capability_spec_strings.size()); + + return parameters; + } + + virtual void process_response(typename capabilities2_msgs::srv::GetCapabilitySpecs::Response::SharedPtr response) override + { + // This function can be used to log the response or perform any additional processing if needed + RCLCPP_INFO(node_->get_logger(), "GetCapabilitySpecsRunner received response with %zu capability specs", response->capability_specs.size()); + } +}; + +} // namespace capabilities2_runner diff --git a/capabilities2_runner/include/capabilities2_runner/system/input_multiplex_runner.hpp b/capabilities2_runner/include/capabilities2_runner/system/input_multiplex_runner.hpp new file mode 100644 index 0000000..c7d3ad1 --- /dev/null +++ b/capabilities2_runner/include/capabilities2_runner/system/input_multiplex_runner.hpp @@ -0,0 +1,108 @@ +#pragma once + +#include + +namespace capabilities2_runner +{ +class InputMultiplexRunner : public ThreadTriggerRunner +{ +public: + /** + * @brief Constructor which needs to be empty due to plugin semantics + */ + InputMultiplexRunner() : ThreadTriggerRunner() + { + } + + /** + * @brief Starter function for starting the action runner + * + * @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, const std::string& bond_id) override + { + init_base(node, run_config); + + // emit started event + emit_started(bond_id, "", param_on_started()); + } + + /** + * @brief stop function to cease functionality and shutdown + * + */ + virtual void stop(const std::string& bond_id, const std::string& instance_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, instance_id, param_on_stopped()); + + RCLCPP_INFO(node_->get_logger(), "stopping runner"); + } + +protected: + /** + * @brief Trigger process to be executed. + * + * @param parameters pointer to tinyxml2::XMLElement that contains parameters + * @param thread_id unique identifier for the execution thread + */ + virtual void execution(capabilities2_events::EventParameters parameters, const std::string& thread_id) override + { + // split thread_id to get bond_id and instance_id (format: "bond_id/instance_id") + std::string bond_id = ThreadTriggerRunner::bond_from_thread_id(thread_id); + std::string instance_id = ThreadTriggerRunner::instance_from_thread_id(thread_id); + + int input_count = std::any_cast(parameters.get_value("input_count", 1)); + + // track the input count for the runner_id + if (input_count_tracker.find(instance_id) == input_count_tracker.end()) + { + input_count_tracker[instance_id] = 1; + expected_input_count[instance_id] = input_count; + } + else + { + input_count_tracker[instance_id] += 1; + } + + // check if the input count has reached the expected input count for the instance_id and if so execute the process + if (input_count_tracker[instance_id] == expected_input_count[instance_id]) + { + RCLCPP_INFO(node_->get_logger(), + "instance_id: %s has received all expected inputs. Executing process for instance_id: %s", + instance_id.c_str(), instance_id.c_str()); + + // If on_success is defined, emit success event will trigger it. If not defined, it will be a no-op. + emit_succeeded(bond_id, instance_id, param_on_success()); + + RCLCPP_INFO(node_->get_logger(), "execution successful for instance_id: %s", instance_id.c_str()); + } + else + { + RCLCPP_INFO(node_->get_logger(), + "instance_id: %s pending expected inputs. Current count: %d/%d for instance_id: %s", instance_id.c_str(), + input_count_tracker[instance_id], expected_input_count[instance_id], instance_id.c_str()); + } + + RCLCPP_INFO(node_->get_logger(), "multiplexing complete. Thread closing for instance_id: %s", instance_id.c_str()); + } + +protected: + // input count tracker + std::map input_count_tracker; + + // expected input count + std::map expected_input_count; + + // completed executions + std::map completed_executions; +}; + +} // namespace capabilities2_runner diff --git a/capabilities2_runner/include/capabilities2_runner/threadtrigger_runner.hpp b/capabilities2_runner/include/capabilities2_runner/threadtrigger_runner.hpp new file mode 100644 index 0000000..f51b2c7 --- /dev/null +++ b/capabilities2_runner/include/capabilities2_runner/threadtrigger_runner.hpp @@ -0,0 +1,153 @@ +#pragma once + +#include +#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 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) + { + // 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 instance_id from thread_id + * + * @param thread_id unique identifier for the execution thread, format: "bond_id/instance_id" + * @return const std::string + */ + static const std::string instance_from_thread_id(const std::string& thread_id) + { + // extract instance_id from thread_id (format: "bond_id/instance_id") + size_t slash_pos = thread_id.find('/'); + std::string instance_id = (slash_pos != std::string::npos) ? thread_id.substr(slash_pos + 1) : ""; + return instance_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 + * @param instance_id unique identifier for the instance associated with this runner trigger event + */ + virtual void trigger(capabilities2_events::EventParameters& parameters, const std::string& bond_id, + const std::string& instance_id) override + { + // 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 + "/" + instance_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(&ThreadTriggerRunner::execution, this, parameters, thread_id); + } + + // emit trigger event + emit_event(bond_id, instance_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 thread id: %s", thread_id.c_str()); + } + +protected: + /** + * @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 + * + * @attention: Should be implemented on derieved classes and should call success and failure events appropriately + */ + virtual void execution(capabilities2_events::EventParameters 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()) + { + // TODO: FIX THIS FUTURE MICHAEL + exec_thread.join(); + } + } + + // 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..795279e 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,71 @@ 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, const std::string& instance_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 subscription is null + // this can happen if the runner is not able to find the topic resource + + if (!subscription_) + throw runner_exception("cannot stop runner subscriber that was not started"); + + // emit stop event + emit_stopped(bond_id, instance_id, param_on_stopped()); + + 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(capabilities2_events::EventParameters parameters, const std::string& thread_id) override { - // if parameters are not provided then cannot proceed - if (!parameters_[id]) - throw runner_exception("cannot grab data without parameters"); + // split thread_id to get bond_id and instance_id (format: "bond_id/instance_id") + std::string bond_id = ThreadTriggerRunner::bond_from_thread_id(thread_id); + std::string instance_id = ThreadTriggerRunner::instance_from_thread_id(thread_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)); - } + // emit started event + emit_started(bond_id, instance_id, param_on_started()); - std::unique_lock lock(mutex_); + // block until a message is received in the callback and stored in latest_message_ + std::unique_lock lock(block_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, instance_id, param_on_success()); } 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)); - } - } - - 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)); + RCLCPP_ERROR(node_->get_logger(), "Message receiving failed."); + // emit failed event + emit_failed(bond_id, instance_id, param_on_failure()); } - 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: @@ -148,8 +125,22 @@ class TopicRunner : public RunnerBase cv_.notify_all(); } + /** + * @brief parameter to be used in the on_started event emission + */ typename rclcpp::Subscription::SharedPtr subscription_; + /** + * @brief parameter to store the latest message received in the callback for use in the trigger execution + */ mutable typename TopicT::SharedPtr latest_message_; + + /** + * @brief condition variable and mutex for synchronizing the callback and trigger execution + */ + std::condition_variable cv_; + std::mutex block_mutex_; + bool completed_; }; -} // namespace capabilities2_runner \ No newline at end of file + +} // namespace capabilities2_runner diff --git a/capabilities2_runner/interfaces/GetCapabilitySpecsRunner.yaml b/capabilities2_runner/interfaces/GetCapabilitySpecsRunner.yaml new file mode 100644 index 0000000..3897e56 --- /dev/null +++ b/capabilities2_runner/interfaces/GetCapabilitySpecsRunner.yaml @@ -0,0 +1,34 @@ +%YAML 1.1 +--- +name: GetCapabilitySpecsRunner +spec_version: 1.1 +spec_type: interface +description: "This capability depends on the capabilities2 server functionalities and allows an decision making authority such as an LLM extract + capabilities of the robot. The capability can be trigged with an command such as + ''. + This is included in the default starting plan and the decision making authority such as an LLM does not need to include this in any + generated plans. The details for the Capaiblities are stored in a format as follows, + ' + + + name: Navigation + dependencies: + - MapServer + - PathPlanner + + + + + name: ObjectDetection + dependencies: + - Camera + - ImageProcessor + + + ' " + +interface: + services: + "/capabilities/get_capability_specs": + type: "capabilities2_msgs::srv::GetCapabilitySpecs" + description: "This capability focuses on extracting robot's capabilities and transfering them to decision making authority." \ No newline at end of file diff --git a/capabilities2_runner/interfaces/InputMultiplexRunner.yaml b/capabilities2_runner/interfaces/InputMultiplexRunner.yaml new file mode 100644 index 0000000..854c60e --- /dev/null +++ b/capabilities2_runner/interfaces/InputMultiplexRunner.yaml @@ -0,0 +1,13 @@ +%YAML 1.1 +--- +name: InputMultiplexRunner +spec_version: 1.1 +spec_type: interface +description: "This capability combines the results of all input by multiplexing events into a single interface. It allows the robot to wait or + multiple parallel processes at once until completion. This is inserted by the system itself and a decision making authority such + as an LLM does not need to include this in plans generated by it." +interface: + actions: + "empty": + type: std_srvs/action/Empty + description: empty. not used \ No newline at end of file diff --git a/capabilities2_runner/package.xml b/capabilities2_runner/package.xml index f33f924..4fc4f5a 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,15 +21,29 @@ pluginlib std_msgs capabilities2_msgs - capabilities2_utils - event_logger - event_logger_msgs - tinyxml2_vendor + capabilities2_events - - uuid + tinyxml2_vendor ament_cmake + + + + interfaces/InputMultiplexRunner.yaml + + + + providers/InputMultiplexRunner.yaml + + + + + interfaces/GetCapabilitySpecsRunner.yaml + + + + providers/GetCapabilitySpecsRunner.yaml + diff --git a/capabilities2_runner/plugins.xml b/capabilities2_runner/plugins.xml index 756596f..8da5018 100644 --- a/capabilities2_runner/plugins.xml +++ b/capabilities2_runner/plugins.xml @@ -1,4 +1,10 @@ + + + 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 +15,9 @@ A plugin that provides a dummy-runner to test capabilities2 + + + A plugin that request capabilities from the capabilities server + + diff --git a/capabilities2_runner/providers/GetCapabilitySpecsRunner.yaml b/capabilities2_runner/providers/GetCapabilitySpecsRunner.yaml new file mode 100644 index 0000000..af430f6 --- /dev/null +++ b/capabilities2_runner/providers/GetCapabilitySpecsRunner.yaml @@ -0,0 +1,9 @@ +# the empty provider +%YAML 1.1 +--- +name: GetCapabilitySpecsRunner +spec_type: provider +spec_version: 1.1 +description: "The capability provider for extracting capabilities from server. This is used to identify the capabilities of the robot" +implements: capabilities2_runner/GetCapabilitySpecsRunner +runner: capabilities2_runner::GetCapabilitySpecsRunner \ No newline at end of file diff --git a/capabilities2_runner/providers/InputMultiplexRunner.yaml b/capabilities2_runner/providers/InputMultiplexRunner.yaml new file mode 100644 index 0000000..23403a1 --- /dev/null +++ b/capabilities2_runner/providers/InputMultiplexRunner.yaml @@ -0,0 +1,8 @@ +%YAML 1.1 +--- +name: InputMultiplexRunner +spec_type: provider +spec_version: 1.1 +description: "The capability provider for the capabilities2_runner/InputMultiplexRunner interface" +implements: capabilities2_runner/InputMultiplexRunner +runner: capabilities2_runner::InputMultiplexRunner \ No newline at end of file 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_runner/src/dummy_runner.cpp b/capabilities2_runner/src/dummy_runner.cpp new file mode 100644 index 0000000..644a1d6 --- /dev/null +++ b/capabilities2_runner/src/dummy_runner.cpp @@ -0,0 +1,57 @@ +#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: + 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, "", param_on_started()); + } + + void stop(const std::string& bond_id, const std::string& instance_id = "") override + { + // guard node + if (!node_) + throw runner_exception("node not initialized"); + + // stop the runner + RCLCPP_INFO(node_->get_logger(), "Dummy runner stopped"); + + // emit stopped event + emit_stopped(bond_id, instance_id, param_on_stopped()); + } + + void trigger(capabilities2_events::EventParameters& parameters, const std::string& bond_id, + const std::string& instance_id = "") override + { + RCLCPP_WARN(node_->get_logger(), "Dummy runner cannot trigger"); + + // emit failed event + emit_failed(bond_id, instance_id, param_on_failure()); + + // throw an exception as this runner does not support trigger execution + throw runner_exception("Dummy runner does not support trigger execution"); + } +}; + +} // namespace capabilities2_runner + +// 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..912983b --- /dev/null +++ b/capabilities2_runner/src/launch_runner.cpp @@ -0,0 +1,6 @@ +#include +#include +#include + +// register runner plugins +PLUGINLIB_EXPORT_CLASS(capabilities2_runner::LaunchRunner, capabilities2_runner::RunnerBase) diff --git a/capabilities2_runner/src/standard_runners.cpp b/capabilities2_runner/src/standard_runners.cpp deleted file mode 100644 index 58ca354..0000000 --- a/capabilities2_runner/src/standard_runners.cpp +++ /dev/null @@ -1,48 +0,0 @@ -#include -#include -#include - -namespace capabilities2_runner -{ -class DummyRunner : public RunnerBase -{ -public: - void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config) override - { - // init the base runner - init_base(node, run_config); - - // do nothing - RCLCPP_INFO(node_->get_logger(), "Dummy runner started"); - } - - void stop() override - { - // guard node - if (!node_) - throw runner_exception("node not initialized"); - - // 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"); - } - -protected: - // throw on triggerExecution function - void execution(int id) override - { - RCLCPP_INFO(node_->get_logger(), "Dummy runner does not have triggerExecution()"); - } -}; - -} // 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/system/system_runners.cpp b/capabilities2_runner/src/system/system_runners.cpp new file mode 100644 index 0000000..c6f2a47 --- /dev/null +++ b/capabilities2_runner/src/system/system_runners.cpp @@ -0,0 +1,8 @@ +#include +#include +#include +#include + +// register runner plugins +PLUGINLIB_EXPORT_CLASS(capabilities2_runner::GetCapabilitySpecsRunner, capabilities2_runner::RunnerBase); +PLUGINLIB_EXPORT_CLASS(capabilities2_runner::InputMultiplexRunner, capabilities2_runner::RunnerBase); \ No newline at end of file diff --git a/capabilities2_server/CMakeLists.txt b/capabilities2_server/CMakeLists.txt index bc9f429..577cd70 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,16 +18,13 @@ 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) +# find_package(PkgConfig) +# pkg_check_modules(UUID REQUIRED uuid) # Find SQLite3 find_package(SQLite3) @@ -38,7 +35,7 @@ find_package(yaml-cpp REQUIRED) include_directories(include ${SQLITE3_INCLUDE_DIRS} ${YAML_CPP_INCLUDE_DIRS} - ${UUID_INCLUDE_DIRS} + # ${UUID_INCLUDE_DIRS} ) ####################################################################### @@ -52,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 @@ -62,14 +59,12 @@ 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 - UUID + # UUID ) rclcpp_components_register_node(${PROJECT_NAME}_comp @@ -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 @@ -95,7 +90,7 @@ add_executable(${PROJECT_NAME}_node ) target_link_libraries(${PROJECT_NAME}_node - ${UUID_LIBRARIES} + # ${UUID_LIBRARIES} ${SQLITE3_LIBRARIES} ${YAML_CPP_LIBRARIES} ) @@ -106,14 +101,12 @@ 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 - UUID + # UUID ) install(TARGETS ${PROJECT_NAME}_node @@ -128,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/bond_cache.hpp b/capabilities2_server/include/capabilities2_server/bond_cache.hpp index 4705371..faaf39c 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); @@ -111,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) diff --git a/capabilities2_server/include/capabilities2_server/capabilities_api.hpp b/capabilities2_server/include/capabilities2_server/capabilities_api.hpp index e7a246a..7413366 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 @@ -16,10 +15,9 @@ #include #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()); } /** @@ -69,6 +66,7 @@ class CapabilitiesAPI * @param node ros node pointer of the ros server * @param capability capability name to be started * @param provider provider of the capability + * @param bond_id bond_id of the capability instance to be started * * @return `true` if capability started successfully. else returns `false` */ @@ -84,7 +82,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,44 +103,24 @@ 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. * * @param capability capability name to be stopped + * @param bond_id bond_id of the capability instance to be stopped */ void stop_capability(const std::string& capability) { @@ -150,7 +128,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 +142,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 +162,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 +186,44 @@ 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 + * @param instance_id + */ + void trigger_capability(const std::string& bond_id, const std::string& capability, const std::string& instance_id, + capabilities2_events::EventParameters parameters) + { + // 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(bond_id, capability, instance_id, parameters); + } + 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 +246,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& bond_id, const capabilities2_msgs::msg::CapabilityConnection& connection) { + // 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)) + { + 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; + } + try { - // log - event_->info("Setting triggers for capability: " + capability); + // create a unique connection id for the connection using the bond_id and instance ids + //(format: "bond_id/instance_id/target_instance_id") + const std::string connection_id = bond_id + '/' + connection.source.instance_id + '/' + connection.target.instance_id; - runner_cache_.set_runner_triggers(capability, event_options); + // 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 +312,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 +324,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 +332,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 +341,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 +353,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 +361,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 +369,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 +381,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 +389,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 +411,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 +624,8 @@ class CapabilitiesAPI return running_capabilities; } + /** */ + /** * @brief get pid of capability * @@ -600,7 +642,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 @@ -614,13 +656,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); @@ -632,22 +674,6 @@ class CapabilitiesAPI } } -public: - /** - * @brief generate a unique bond id - * - * @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); - } - private: // bind a dependency to an internal bond // this is a bond without a live external connection @@ -657,7 +683,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 = capabilities2_events::UUIDGenerator::gen_uuid_str(); // add the bond id to the bond cache bond_cache_.add_bond(capability, bond_id); @@ -690,12 +717,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/include/capabilities2_server/capabilities_server.hpp b/capabilities2_server/include/capabilities2_server/capabilities_server.hpp index e5e5b30..2fec6b5 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,54 @@ 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.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; + } + + // make sure instance id is provided + if (req->capability.instance_id.empty()) + { + RCLCPP_ERROR(get_logger(), "trigger_capability: instance_id is empty"); + return; + } + + // try triggering capability // TODO: handle errors - trigger_capability(req->capability, req->parameters); + trigger_capability(req->bond_id, req->capability.capability, req->capability.instance_id, + capabilities2_events::EventParameters(req->capability)); // 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 +315,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 +344,35 @@ 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; + // need to have a bond established + if (req->bond_id.empty()) + { + RCLCPP_ERROR(get_logger(), "connect_capability: bond_id is empty"); + return; + } - 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 instance id + if (req->connection.source.instance_id.empty()) + { + RCLCPP_ERROR(get_logger(), "connect_capability: instance_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 target instance id + if (req->connection.target.instance_id.empty()) + { + RCLCPP_ERROR(get_logger(), "connect_capability: target_instance_id is empty"); + return; + } - // setup triggers between parameters - set_triggers(req->source.capability, event_options); + // api connect capability + connect_capability(req->bond_id, req->connection); // response is empty } @@ -368,7 +384,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 +412,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 +434,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 +477,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 +509,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 +517,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 +530,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 +539,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 +570,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 +591,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 +599,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 +612,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 +621,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 +652,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 +729,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 +741,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_; diff --git a/capabilities2_server/include/capabilities2_server/models/provider.hpp b/capabilities2_server/include/capabilities2_server/models/provider.hpp index c90b4f2..eb64573 100644 --- a/capabilities2_server/include/capabilities2_server/models/provider.hpp +++ b/capabilities2_server/include/capabilities2_server/models/provider.hpp @@ -6,6 +6,7 @@ #include #include #include +#include #include namespace capabilities2_server @@ -21,7 +22,7 @@ namespace models * the provider can be specific to a robot implementation of a general capability * */ -struct provider_model_t : public remappable_base_t, predicateable_base_t, defineable_base_t +struct provider_model_t : public remappable_base_t, public predicateable_base_t, public defineable_base_t { header_model_t header; std::string implements; 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/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/include/capabilities2_server/runner_cache.hpp b/capabilities2_server/include/capabilities2_server/runner_cache.hpp index db7c9fe..2689b4e 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 * @@ -77,7 +70,7 @@ class RunnerCache } // start the runner - runner_cache_[capability]->start(node, run_config.to_runner_opts()); + runner_cache_[capability]->start(node, run_config.to_runner_opts(), ""); } /** @@ -86,43 +79,71 @@ 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 + * @param instance_id unique identifier for the instance of the capability */ - void trigger_runner(const std::string& capability, const std::string& parameters) + void trigger_runner(const std::string& bond_id, const std::string& capability, const std::string& instance_id, + capabilities2_events::EventParameters parameters) { + // 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, instance_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 + * @param event_emitter event emitter to be used by this runner for emitting events on state changes to the target + * capability */ - 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, instance_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, std::placeholders::_4)); + } + catch (const capabilities2_events::event_exception& e) + { + throw capabilities2_runner::runner_exception(e.what()); + } } /** * @brief Remove a given runner * * @param capability capability to be removed + * @param bond_id bond_id of the capability instance to be removed + * + * This will stop the runner and remove it from the cache. If the runner is not found then an error is thrown. */ void remove_runner(const std::string& capability) { @@ -135,7 +156,7 @@ class RunnerCache // safely stop the runner try { - runner_cache_[capability]->stop(); + runner_cache_[capability]->stop(""); } catch (const capabilities2_runner::runner_exception& e) { @@ -147,7 +168,7 @@ class RunnerCache // runner_cache_[capability].reset(); // remove the runner from map - // runner_cache_.erase(capability); + runner_cache_.erase(capability); } /** @@ -226,12 +247,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 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( diff --git a/capabilities2_server/package.xml b/capabilities2_server/package.xml index 3efffc8..687fdad 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 - diff --git a/capabilities2_server/readme.md b/capabilities2_server/readme.md index 4b81b3a..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 @@ -46,25 +45,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 diff --git a/capabilities2_server/test/test.cpp b/capabilities2_server/test/test.cpp index 68fe67a..d71963c 100644 --- a/capabilities2_server/test/test.cpp +++ b/capabilities2_server/test/test.cpp @@ -32,7 +32,7 @@ int main(int argc, char** argv) spec.type = capabilities2_msgs::msg::CapabilitySpec::CAPABILITY_PROVIDER; spec.content = data; - for (const auto& s : node.get_sematic_interfaces("std_capabilities/empty")) + for (const auto& s : node.get_semantic_interfaces("std_capabilities/empty")) { std::cout << s << std::endl; } 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 - - 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