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 9148ff5..561d1a2 100644 --- a/.devcontainer/devcontainer.json +++ b/.devcontainer/devcontainer.json @@ -1,17 +1,10 @@ { - "name": "jazzy base", + "name": "capabilities2 jazzy base", "dockerFile": "Dockerfile", "remoteUser": "ubuntu", - "containerEnv": { - "DISPLAY": "${localEnv:DISPLAY}" - }, "runArgs": [ - "--privileged", "--network=host" ], "workspaceMount": "source=${localWorkspaceFolder},target=/home/ubuntu/colcon_ws/src/${localWorkspaceFolderBasename},type=bind", - "workspaceFolder": "/home/ubuntu/colcon_ws", - "mounts": [ - "source=${localEnv:HOME}${localEnv:USERPROFILE}/.bash_history,target=/home/ubuntu/.bash_history,type=bind" - ] + "workspaceFolder": "/home/ubuntu/colcon_ws" } diff --git a/.devcontainer/humble/Dockerfile b/.devcontainer/humble/Dockerfile new file mode 100644 index 0000000..a71f968 --- /dev/null +++ b/.devcontainer/humble/Dockerfile @@ -0,0 +1,30 @@ +FROM ros:humble + +# Add vscode user with same UID and GID as your host system +# (modified from https://code.visualstudio.com/remote/advancedcontainers/add-nonroot-user#_creating-a-nonroot-user) +# ubuntu now has a 1000 user ubuntu +ARG USERNAME=ubuntu +ARG USER_UID=1000 +ARG USER_GID=$USER_UID + +RUN groupadd --gid $USER_GID $USERNAME \ + && useradd --uid $USER_UID --gid $USER_GID -m $USERNAME + +RUN apt-get update && apt-get upgrade -y \ + && apt-get install -y \ + sudo \ + git \ + && echo $USERNAME ALL=\(root\) NOPASSWD:ALL > /etc/sudoers.d/$USERNAME \ + && chmod 0440 /etc/sudoers.d/$USERNAME + +# Switch from root to user +USER $USERNAME + +# Rosdep update +RUN rosdep update + +# Source the ROS setup file +RUN echo "source /opt/ros/${ROS_DISTRO}/setup.bash" >> ~/.bashrc + +# make sure folders exist +RUN mkdir -p ~/colcon_ws/src diff --git a/.devcontainer/humble/devcontainer.json b/.devcontainer/humble/devcontainer.json new file mode 100644 index 0000000..02e20ce --- /dev/null +++ b/.devcontainer/humble/devcontainer.json @@ -0,0 +1,17 @@ +{ + "name": "humble base", + "dockerFile": "Dockerfile", + "remoteUser": "ubuntu", + "containerEnv": { + "DISPLAY": "${localEnv:DISPLAY}" + }, + "runArgs": [ + "--privileged", + "--network=host" + ], + "workspaceMount": "source=${localWorkspaceFolder},target=/home/ubuntu/colcon_ws/src/${localWorkspaceFolderBasename},type=bind", + "workspaceFolder": "/home/ubuntu/colcon_ws", + "mounts": [ + "source=${localEnv:HOME}${localEnv:USERPROFILE}/.bash_history,target=/home/ubuntu/.bash_history,type=bind" + ] +} diff --git a/AUTHORS b/AUTHORS index 848e7ac..4a5fb23 100644 --- a/AUTHORS +++ b/AUTHORS @@ -1,4 +1,4 @@ Michael Pritchard Thomas Muller-dardelin Kalana Rathnayake -Buddhi Gamage +Buddhi Gamage \ No newline at end of file diff --git a/TODO.md b/TODO.md index 0a7e405..219be4d 100644 --- a/TODO.md +++ b/TODO.md @@ -4,14 +4,32 @@ - [x] names need to be in package/name format everywhere - [x] better docs -- [ ] BUG: escape db function variables +- [x] BUG: handle "'" in db queries +- [x] BUG: escape db function variables - [ ] close or change communication to launch proxy so that it can't be accessed from ros network -- [ ] BUG: handle "'" in db queries +- [ ] 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 +- [x] try using ros package to find exports automatically +- [x] improve the event system - [ ] implement provider definition handling in runner -- [ ] try using ros package to find exports automatically -- [ ] improve the event system - [ ] 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/CMakeLists.txt b/capabilities2/CMakeLists.txt index cada8ad..43607f2 100644 --- a/capabilities2/CMakeLists.txt +++ b/capabilities2/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 3.10) +cmake_minimum_required(VERSION 3.16) project(capabilities2) find_package(ament_cmake REQUIRED) ament_package() diff --git a/capabilities2/package.xml b/capabilities2/package.xml index 691851b..79d0029 100644 --- a/capabilities2/package.xml +++ b/capabilities2/package.xml @@ -1,24 +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_executor/.clang-format b/capabilities2_events/.clang-format similarity index 100% rename from capabilities2_executor/.clang-format rename to capabilities2_events/.clang-format diff --git a/capabilities2_runner_audio/CMakeLists.txt b/capabilities2_events/CMakeLists.txt similarity index 55% rename from capabilities2_runner_audio/CMakeLists.txt rename to capabilities2_events/CMakeLists.txt index 15ba918..fb5ead3 100644 --- a/capabilities2_runner_audio/CMakeLists.txt +++ b/capabilities2_events/CMakeLists.txt @@ -1,5 +1,5 @@ -cmake_minimum_required(VERSION 3.8) -project(capabilities2_runner_audio) +cmake_minimum_required(VERSION 3.16) +project(capabilities2_events) # Default to C++17 if(NOT CMAKE_CXX_STANDARD) @@ -13,50 +13,42 @@ endif() # find dependencies find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) -find_package(rclcpp_action REQUIRED) -find_package(pluginlib REQUIRED) -find_package(hri_audio_msgs REQUIRED) find_package(capabilities2_msgs REQUIRED) -find_package(capabilities2_runner REQUIRED) + +# uuid find_package(PkgConfig) -pkg_check_modules(TINYXML2 tinyxml2 REQUIRED) +pkg_check_modules(UUID REQUIRED uuid) -include_directories(include -${TINYXML2_INCLUDE_DIRS} -) +include_directories(include ${UUID_INCLUDE_DIRS}) +# build library add_library(${PROJECT_NAME} SHARED - src/audio_runners.cpp + src/uuid_generator.cpp ) -target_link_libraries(${PROJECT_NAME} - ${TINYXML2_LIBRARIES} -) +target_link_libraries(${PROJECT_NAME} ${UUID_LIBRARIES}) ament_target_dependencies(${PROJECT_NAME} rclcpp - rclcpp_action - pluginlib - hri_audio_msgs capabilities2_msgs - capabilities2_runner - TINYXML2 + UUID ) -pluginlib_export_plugin_description_file(${PROJECT_NAME} plugins.xml) +# install headers +install(DIRECTORY include/ + DESTINATION include +) +# install library install(TARGETS ${PROJECT_NAME} - EXPORT ${PROJECT_NAME} + EXPORT export_${PROJECT_NAME} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin ) -install(DIRECTORY include/ - DESTINATION include -) - 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_runner_bt/package.xml b/capabilities2_events/package.xml similarity index 53% rename from capabilities2_runner_bt/package.xml rename to capabilities2_events/package.xml index 2c84b98..e26ef65 100644 --- a/capabilities2_runner_bt/package.xml +++ b/capabilities2_events/package.xml @@ -1,31 +1,24 @@ - capabilities2_runner_bt - 0.0.1 + capabilities2_events + 0.2.0 - Package for behaviour tree based capabilities2 runners + Event subsystem for the capabilities2 framework Michael Pritchard - mik-p + Kalana Ratnayake Michael Pritchard + Kalana Ratnayake MIT ament_cmake rclcpp - rclcpp_action - pluginlib - std_msgs capabilities2_msgs - capabilities2_runner - behaviortree_cpp - - uuid - tinyxml2 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_executor/CMakeLists.txt b/capabilities2_executor/CMakeLists.txt deleted file mode 100644 index ccec815..0000000 --- a/capabilities2_executor/CMakeLists.txt +++ /dev/null @@ -1,69 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(capabilities2_executor) - -# 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(rclcpp_action REQUIRED) -find_package(capabilities2_msgs REQUIRED) - -find_package(PkgConfig) -pkg_check_modules(TINYXML2 tinyxml2 REQUIRED) - -include_directories( - include - ${TINYXML2_INCLUDE_DIRS} -) - -add_library(${PROJECT_NAME} SHARED - src/capabilities_executor.cpp -) - -target_link_libraries(${PROJECT_NAME} - ${TINYXML2_LIBRARIES} -) - -ament_target_dependencies(${PROJECT_NAME} - rclcpp - rclcpp_action - capabilities2_msgs - TINYXML2 -) - -add_library(${PROJECT_NAME}_file SHARED - src/capabilities_file_parser.cpp -) - -target_link_libraries(${PROJECT_NAME}_file - ${TINYXML2_LIBRARIES} -) - -ament_target_dependencies(${PROJECT_NAME}_file - rclcpp - rclcpp_action - capabilities2_msgs - TINYXML2 -) - -install(DIRECTORY include/ - DESTINATION include -) - -install(DIRECTORY launch - DESTINATION share/${PROJECT_NAME} -) - -install(DIRECTORY config - DESTINATION share/${PROJECT_NAME} -) - -ament_package() diff --git a/capabilities2_executor/config/.gitkeep b/capabilities2_executor/config/.gitkeep deleted file mode 100644 index e69de29..0000000 diff --git a/capabilities2_executor/include/capabilities2_executor/capabilities_executor.hpp b/capabilities2_executor/include/capabilities2_executor/capabilities_executor.hpp deleted file mode 100644 index 5624987..0000000 --- a/capabilities2_executor/include/capabilities2_executor/capabilities_executor.hpp +++ /dev/null @@ -1,531 +0,0 @@ -#include -#include -#include -#include -#include -#include - -#include -#include - -#include - -#include - -#include -#include - -#include -#include -#include -#include - -/** - * @brief Capabilities Executor - * - * Capabilities executor node that provides a ROS client for the capabilities server. - * Able to receive a XML file that implements a plan via action server that it exposes - * or via a file read. - * - */ - -class CapabilitiesExecutor : public rclcpp::Node -{ -public: - CapabilitiesExecutor(const rclcpp::NodeOptions& options = rclcpp::NodeOptions()) - : Node("Capabilities2_Executor", options) - { - control_tag_list.push_back("sequential"); - control_tag_list.push_back("parallel"); - control_tag_list.push_back("recovery"); - - this->planner_server_ = rclcpp_action::create_server( - this, "~/capabilities", - std::bind(&CapabilitiesExecutor::handle_goal, this, std::placeholders::_1, std::placeholders::_2), - std::bind(&CapabilitiesExecutor::handle_cancel, this, std::placeholders::_1), - std::bind(&CapabilitiesExecutor::handle_accepted, this, std::placeholders::_1)); - - this->client_capabilities_ = rclcpp_action::create_client(this, "~/capabilities_fabric"); - - get_interfaces_client_ = this->create_client("~/get_interfaces"); - get_sem_interf_client_ = this->create_client("~/get_semantic_interfaces"); - - establish_bond_client_ = this->create_client("~/establish_bond"); - } - -private: - /** - * @brief Handle the goal request that comes in from client. returns whether goal is accepted or rejected - * - * - * @param uuid uuid of the goal - * @param goal pointer to the action goal message - * @return rclcpp_action::GoalResponse - */ - rclcpp_action::GoalResponse handle_goal(const rclcpp_action::GoalUUID& uuid, - std::shared_ptr goal) - { - RCLCPP_INFO(this->get_logger(), "Received the goal request with the plan"); - (void)uuid; - - // try to parse the std::string plan from capabilities_msgs/Plan to the to a XMLDocument file - tinyxml2::XMLError xml_status = document.Parse(goal->plan.c_str()); - - // check if the file parsing failed - if (xml_status != tinyxml2::XMLError::XML_SUCCESS) - { - RCLCPP_INFO(this->get_logger(), "Parsing the plan from goal message failed"); - return rclcpp_action::GoalResponse::REJECT; - } - - return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; - } - - /** - * @brief Handle the goal cancel request that comes in from client. - * - * @param goal_handle pointer to the action goal handle - * @return rclcpp_action::GoalResponse - */ - rclcpp_action::CancelResponse - handle_cancel(const std::shared_ptr> goal_handle) - { - RCLCPP_INFO(this->get_logger(), "Received the request to cancel the plan"); - (void)goal_handle; - - return rclcpp_action::CancelResponse::ACCEPT; - } - - /** - * @brief Handle the goal accept event originating from handle_goal. - * - * @param goal_handle pointer to the action goal handle - */ - void - handle_accepted(const std::shared_ptr> goal_handle) - { - execution_thread = std::make_unique( - std::bind(&CapabilitiesExecutor::execute_plan, this, std::placeholders::_1), goal_handle); - } - - /** - * @brief request the interfaces, semantic_interfaces and providers from the capabilities2 server - * - * @return `true` if interface retreival is successful,`false` otherwise - */ - bool request_information() - { - // create request messages - auto request_interface = std::make_shared(); - auto request_sematic = std::make_shared(); - auto request_providers = std::make_shared(); - - // request data from the server - auto result_future = get_interfaces_client_->async_send_request(request_interface); - - RCLCPP_INFO(this->get_logger(), "Requesting Interface information from Capabilities2 Server"); - - // wait until data is received - if (rclcpp::spin_until_future_complete(shared_from_this(), result_future) != rclcpp::FutureReturnCode::SUCCESS) - { - RCLCPP_INFO(this->get_logger(), "Failed to receive Interface information from Capabilities2 Server"); - return false; - } - - RCLCPP_INFO(this->get_logger(), "Received Interface information from Capabilities2 Server"); - - // request semantic interfaces available for each and every interface got from the server - for (const auto& interface : result_future.get()->interfaces) - { - request_sematic->interface = interface; - - // request semantic interface from the server - auto result_semantic_future = get_sem_interf_client_->async_send_request(request_sematic); - - RCLCPP_INFO(this->get_logger(), "Requesting Semantic Interface information from Capabilities2 Server for %s", - interface.c_str()); - - // wait until data is received - if (rclcpp::spin_until_future_complete(shared_from_this(), result_semantic_future) != - rclcpp::FutureReturnCode::SUCCESS) - { - RCLCPP_INFO(this->get_logger(), - "Failed to receive Semantic Interface information from Capabilities2 Server for %s", - interface.c_str()); - return false; - } - - RCLCPP_INFO(this->get_logger(), "Received Semantic Interface information from Capabilities2 Server for %s", - interface.c_str()); - - // add sematic interfaces to the list if available - if (result_semantic_future.get()->semantic_interfaces.size() > 0) - { - for (const auto& semantic_interface : result_semantic_future.get()->semantic_interfaces) - { - interface_list.push_back(semantic_interface); - - // request providers of the semantic interface - request_providers->interface = semantic_interface; - request_providers->include_semantic = true; - - auto result_providers_future = get_providers_client_->async_send_request(request_providers); - - RCLCPP_INFO(this->get_logger(), - "Requesting Providers information from Capabilities2 Server for semantic interface %s", - semantic_interface.c_str()); - - // wait until data is received - if (rclcpp::spin_until_future_complete(shared_from_this(), result_providers_future) != - rclcpp::FutureReturnCode::SUCCESS) - { - RCLCPP_INFO(this->get_logger(), "Failed to receive Providers information from Capabilities2 Server for %s", - semantic_interface.c_str()); - return false; - } - - RCLCPP_INFO(this->get_logger(), "Received Providers information from Capabilities2 Server for %s", - semantic_interface.c_str()); - - // add defualt provider to the list - providers_list.push_back(result_providers_future.get()->default_provider); - - // add additional providers to the list if available - if (result_providers_future.get()->providers.size() > 0) - { - for (const auto& provider : result_providers_future.get()->providers) - { - providers_list.push_back(provider); - } - } - } - } - // if no semantic interfaces are availble for a given interface, add the interface instead - else - { - interface_list.push_back(interface); - - // request providers of the semantic interface - request_providers->interface = interface; - request_providers->include_semantic = false; - - auto result_providers_future = get_providers_client_->async_send_request(request_providers); - - RCLCPP_INFO(this->get_logger(), - "Requesting Providers information from Capabilities2 Server for semantic interface %s", - interface.c_str()); - - // wait until data is received - if (rclcpp::spin_until_future_complete(shared_from_this(), result_providers_future) != - rclcpp::FutureReturnCode::SUCCESS) - { - RCLCPP_INFO(this->get_logger(), "Failed to receive Providers information from Capabilities2 Server for %s", - interface.c_str()); - return false; - } - - RCLCPP_INFO(this->get_logger(), "Received Providers information from Capabilities2 Server for %s", - interface.c_str()); - - providers_list.push_back(result_providers_future.get()->default_provider); - - // add sematic interfaces to the list if available - if (result_providers_future.get()->providers.size() > 0) - { - for (const auto& provider : result_providers_future.get()->providers) - { - providers_list.push_back(provider); - } - } - } - } - - return true; - } - - /** - * @brief verify the plan using received interfaces - * - * @return `true` if interface retreival is successful,`false` otherwise - */ - bool verify_plan() - { - // intialize a vector to accomodate elements from both - std::vector tag_list(interface_list.size() + control_tag_list.size()); - std::merge(interface_list.begin(), interface_list.end(), control_tag_list.begin(), control_tag_list.end(), - tag_list.begin()); - - // verify whether document got 'plan' tags - if (!capabilities2_xml_parser::check_plan_tag(document)) - { - RCLCPP_INFO(this->get_logger(), "Execution plan is not compatible. Please recheck and update"); - return false; - } - - // extract the components within the 'plan' tags - tinyxml2::XMLElement* plan = capabilities2_xml_parser::get_plan(document); - - // verify whether the plan is valid - if (!capabilities2_xml_parser::check_tags(plan, interface_list, providers_list, control_tag_list)) - { - RCLCPP_INFO(this->get_logger(), "Execution plan is faulty. Please recheck and update"); - return false; - } - - return true; - } - - /** - * @brief establish the bond with capabilities2 server - * - * @return `true` if bond establishing is successful,`false` otherwise - */ - bool establish_bond() - { - // create bond establishing server request - auto request_bond = std::make_shared(); - - // send the request - auto result_future = establish_bond_client_->async_send_request(request_bond); - - RCLCPP_INFO(this->get_logger(), "Requesting to establish bond with Capabilities2 Server"); - - // wait for the result - if (rclcpp::spin_until_future_complete(shared_from_this(), result_future) != rclcpp::FutureReturnCode::SUCCESS) - { - RCLCPP_INFO(this->get_logger(), "Failed to establish bond with Capabilities2 Server"); - return false; - } - - RCLCPP_INFO(this->get_logger(), "Established bond with Capabilities2 Server"); - - bond_id = result_future.get()->bond_id; - - return true; - } - - /** - * @brief execute the plan - * - * @param server_goal_handle goal handle of the server - */ - void execute_plan( - const std::shared_ptr> server_goal_handle) - { - auto result = std::make_shared(); - - // verify the plan - if (!request_information()) - { - RCLCPP_INFO(this->get_logger(), "Interface retreival failed"); - - // TODO: improve with error codes - result->success = false; - server_goal_handle->canceled(result); - - RCLCPP_INFO(this->get_logger(), "Server Execution Cancelled"); - } - - // verify the plan - if (!verify_plan()) - { - RCLCPP_INFO(this->get_logger(), "Plan verification failed"); - - // TODO: improve with error codes - result->success = false; - server_goal_handle->canceled(result); - - RCLCPP_INFO(this->get_logger(), "Server Execution Cancelled"); - } - - // extract the plan from the XMLDocument - tinyxml2::XMLElement* plan = capabilities2_xml_parser::get_plan(document); - - // Extract the connections from the plan - capabilities2_xml_parser::extract_connections(plan, connection_list); - - // estasblish the bond with the server - if (!establish_bond()) - { - RCLCPP_INFO(this->get_logger(), "Establishing bond failed"); - - // TODO: improve with error codes - result->success = false; - server_goal_handle->canceled(result); - - RCLCPP_INFO(this->get_logger(), "Server Execution Cancelled"); - } - - auto connection_goal_msg = capabilities2_msgs::action::Connections::Goal(); - connection_goal_msg.bond_id = bond_id; - connection_goal_msg.header.stamp = this->get_clock()->now(); - - capabilities2_msgs::msg::CapabilityConnection connection_msg; - - for (const auto& connection : connection_list) - { - connection_msg.source.capability = connection.source_event; - connection_msg.source.provider = connection.source_provider; - connection_msg.target.capability = connection.target_event; - connection_msg.target.provider = connection.target_provider; - - if (connection.connection == capabilities2_executor::connection_type_t::ON_SUCCESS_START) - connection_msg.connection_type = capabilities2_msgs::msg::CapabilityConnection::ON_SUCCESS_START; - - if (connection.connection == capabilities2_executor::connection_type_t::ON_START_START) - connection_msg.connection_type = capabilities2_msgs::msg::CapabilityConnection::ON_START_START; - - if (connection.connection == capabilities2_executor::connection_type_t::ON_FAILURE_START) - connection_msg.connection_type = capabilities2_msgs::msg::CapabilityConnection::ON_FAILURE_START; - - connection_msg.target_parameters = capabilities2_xml_parser::convert_to_string(connection.event_element); - - connection_goal_msg.connections.push_back(connection_msg); - } - - auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); - - // send goal options - // goal response callback - send_goal_options.goal_response_callback = - [this, server_goal_handle]( - const rclcpp_action::ClientGoalHandle::SharedPtr& goal_handle) { - if (!goal_handle) - { - RCLCPP_ERROR(this->get_logger(), "Goal was rejected by server"); - - auto result = std::make_shared(); - - // TODO: improve with error codes - result->success = false; - server_goal_handle->canceled(result); - - RCLCPP_INFO(this->get_logger(), "Server Execution Cancelled"); - } - else - { - RCLCPP_INFO(this->get_logger(), "Goal accepted by server, waiting for result"); - } - }; - - // result callback - send_goal_options.result_callback = - [this, server_goal_handle]( - const rclcpp_action::ClientGoalHandle::WrappedResult& result) { - auto result_out = std::make_shared(); - - switch (result.code) - { - case rclcpp_action::ResultCode::SUCCEEDED: - break; - case rclcpp_action::ResultCode::ABORTED: - RCLCPP_ERROR(this->get_logger(), "Goal was aborted"); - - // TODO: improve with error codes - result_out->success = false; - server_goal_handle->canceled(result_out); - - RCLCPP_INFO(this->get_logger(), "Server Execution Cancelled"); - - return; - case rclcpp_action::ResultCode::CANCELED: - RCLCPP_ERROR(this->get_logger(), "Goal was canceled"); - - // TODO: improve with error codes - result_out->success = false; - server_goal_handle->canceled(result_out); - - RCLCPP_INFO(this->get_logger(), "Server Execution Cancelled"); - - return; - default: - RCLCPP_ERROR(this->get_logger(), "Unknown result code"); - - // TODO: improve with error codes - result_out->success = false; - server_goal_handle->canceled(result_out); - - RCLCPP_INFO(this->get_logger(), "Server Execution Cancelled"); - return; - } - - if (result.result->failed_connections.size() > 0) - { - // TODO: improve with error codes - result_out->success = false; - - for (const auto& failed_connection : result.result->failed_connections) - { - RCLCPP_ERROR(this->get_logger(), "Failed Events : %s", failed_connection.target_parameters.c_str()); - - result_out->failed_elements.push_back(failed_connection.target_parameters); - } - - server_goal_handle->canceled(result_out); - - RCLCPP_ERROR(this->get_logger(), "Server Execution Cancelled"); - } - else - { - // TODO: improve with error codes - result_out->success = true; - server_goal_handle->succeed(result_out); - - RCLCPP_INFO(this->get_logger(), "Server Execution Succeeded"); - } - - rclcpp::shutdown(); - }; - - this->client_capabilities_->async_send_goal(connection_goal_msg, send_goal_options); - } - -private: - /** File Path link */ - std::string plan_file_path; - - /** flag to select loading from file or accepting via action server */ - bool read_file; - - /** Bond ID between capabilities server and this client */ - std::string bond_id; - - /** XML Document */ - tinyxml2::XMLDocument document; - - /** vector of connections */ - std::vector connection_list; - - /** Execution Thread */ - std::shared_ptr execution_thread; - - /** Interface List */ - std::vector interface_list; - - /** Providers List */ - std::vector providers_list; - - /** Control flow List */ - std::vector control_tag_list; - - /** action client for connecting with capabilities server*/ - rclcpp_action::Client::SharedPtr client_capabilities_; - - /** action server that exposes executor*/ - std::shared_ptr> planner_server_; - - /** action server goal handle*/ - std::shared_ptr> goal_handle; - - /** Get interfaces from capabilities server */ - rclcpp::Client::SharedPtr get_interfaces_client_; - - /** Get semantic interfaces from capabilities server */ - rclcpp::Client::SharedPtr get_sem_interf_client_; - - /** Get providers from capabilities server */ - rclcpp::Client::SharedPtr get_providers_client_; - - /** establish bond */ - rclcpp::Client::SharedPtr establish_bond_client_; -}; \ No newline at end of file diff --git a/capabilities2_executor/include/capabilities2_executor/capabilities_file_parser.hpp b/capabilities2_executor/include/capabilities2_executor/capabilities_file_parser.hpp deleted file mode 100644 index 5164897..0000000 --- a/capabilities2_executor/include/capabilities2_executor/capabilities_file_parser.hpp +++ /dev/null @@ -1,133 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include - -#include -#include - -/** - * @brief Capabilities Executor File Parser - * - * Capabilities Executor File Parser node that provides a ROS client for the capabilities executor. - * Will read an XML file that implements a plan and send it to the server - */ - -class CapabilitiesFileParser : public rclcpp::Node -{ -public: - CapabilitiesFileParser(const rclcpp::NodeOptions &options = rclcpp::NodeOptions()) - : Node("Capabilities2_File_Parser", options) - { - declare_parameter("plan_file_path", "plan.xml"); - plan_file_path = get_parameter("plan_file_path").as_string(); - - this->client_ptr_ = rclcpp_action::create_client(this, "~/capabilities"); - - this->timer_ = this->create_wall_timer(std::chrono::milliseconds(500), std::bind(&CapabilitiesFileParser::send_goal, this)); - } - - void send_goal() - { - this->timer_->cancel(); - - // try to load the file - tinyxml2::XMLError xml_status = document.LoadFile(plan_file_path.c_str()); - - // check if the file loading failed - if (xml_status != tinyxml2::XMLError::XML_SUCCESS) - { - RCLCPP_INFO(this->get_logger(), "Loading the file from path : %s failed", plan_file_path.c_str()); - rclcpp::shutdown(); - } - - if (!this->client_ptr_->wait_for_action_server()) - { - RCLCPP_ERROR(this->get_logger(), "Action server not available after waiting"); - rclcpp::shutdown(); - } - - auto goal_msg = capabilities2_msgs::action::Plan::Goal(); - goal_msg.plan = capabilities2_xml_parser::convert_to_string(document); - - RCLCPP_INFO(this->get_logger(), "Sending goal"); - - auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); - - // send goal options - // goal response callback - send_goal_options.goal_response_callback = - [this](const rclcpp_action::ClientGoalHandle::SharedPtr &goal_handle) - { - if (!goal_handle) - { - RCLCPP_ERROR(this->get_logger(), "Goal was rejected by server"); - } - else - { - RCLCPP_INFO(this->get_logger(), "Goal accepted by server, waiting for result"); - } - }; - - // result callback - send_goal_options.result_callback = - [this](const rclcpp_action::ClientGoalHandle::WrappedResult &result) - { - switch (result.code) - { - case rclcpp_action::ResultCode::SUCCEEDED: - break; - case rclcpp_action::ResultCode::ABORTED: - RCLCPP_ERROR(this->get_logger(), "Goal was aborted"); - return; - case rclcpp_action::ResultCode::CANCELED: - RCLCPP_ERROR(this->get_logger(), "Goal was canceled"); - return; - default: - RCLCPP_ERROR(this->get_logger(), "Unknown result code"); - return; - } - - if (result.result->success) - { - RCLCPP_ERROR(this->get_logger(), "Plan executed successfully"); - } - else - { - RCLCPP_ERROR(this->get_logger(), "Plan failed to complete"); - - if (result.result->failed_elements.size() > 0) - { - RCLCPP_ERROR(this->get_logger(), "Plan failed due to incompatible XMLElements in the plan"); - - for (const auto &failed_element : result.result->failed_elements) - RCLCPP_ERROR(this->get_logger(), "Failed Elements : %s", failed_element.c_str()); - } - } - - rclcpp::shutdown(); - }; - - this->client_ptr_->async_send_goal(goal_msg, send_goal_options); - } - -private: - /** File Path link */ - std::string plan_file_path; - - /** XML Document */ - tinyxml2::XMLDocument document; - - /** action client */ - rclcpp_action::Client::SharedPtr client_ptr_; - - /** action server */ - rclcpp::TimerBase::SharedPtr timer_; -}; \ No newline at end of file diff --git a/capabilities2_executor/include/capabilities2_executor/capabilities_xml_parser.hpp b/capabilities2_executor/include/capabilities2_executor/capabilities_xml_parser.hpp deleted file mode 100644 index 102d345..0000000 --- a/capabilities2_executor/include/capabilities2_executor/capabilities_xml_parser.hpp +++ /dev/null @@ -1,265 +0,0 @@ -#include -#include -#include -#include - -namespace capabilities2_xml_parser -{ - /** - * @brief extract elements related plan - * - * @param document XML document to extract plan from - * - * @return plan in the form of tinyxml2::XMLElement* - */ - tinyxml2::XMLElement *get_plan(tinyxml2::XMLDocument &document) - { - return document.FirstChildElement("Plan"); - } - - /** - * @brief search a string in a vector of strings - * - * @param list vector of strings to be searched - * @param value string to be searched in the vector - * - * @return `true` if value is found in list and `false` otherwise - */ - bool search(std::vector list, std::string value) - { - return (std::find(list.begin(), list.end(), value) != list.end()); - } - - /** - * @brief check if the element in question is the last in its level - * - * @param element element to be checked - * - * @return `true` if last and `false` otherwise - */ - bool isLastElement(tinyxml2::XMLElement *element) - { - return (element == element->Parent()->LastChildElement()); - } - - /** - * @brief check if the xml document has valid plan tags - * - * @param document XML document in question - * - * @return `true` if its valid and `false` otherwise - */ - bool check_plan_tag(tinyxml2::XMLDocument &document) - { - std::string plan_tag(document.FirstChildElement()->Name()); - - if (plan_tag == "Plan") - return true; - else - return false; - } - - /** - * @brief convert XMLElement to std::string - * - * @param element element to be converted - * - * @return std::string converted element - */ - std::string convert_to_string(tinyxml2::XMLElement *element) - { - tinyxml2::XMLPrinter printer; - - element->Accept(&printer); - - std::string value = printer.CStr(); - - return value; - } - - /** - * @brief convert XMLDocument to std::string - * - * @param document element to be converted - * - * @return std::string converted document - */ - std::string convert_to_string(tinyxml2::XMLDocument &document) - { - tinyxml2::XMLPrinter printer; - - document.Accept(&printer); - - std::string value = printer.CStr(); - - return value; - } - - /** - * @brief check the plan for invalid/unsupported control and event tags - * uses recursive approach to go through the plan - * - * @param element XML Element to be evaluated - * @param events_list list containing valid event tags - * @param providers_list list containing providers - * @param control_list list containing valid control tags - * - * @return `true` if element valid and supported and `false` otherwise - */ - bool check_tags(tinyxml2::XMLElement *element, - std::vector &events_list, - std::vector &providers_list, - std::vector &control_list) - { - const char **name; - const char **provider; - - element->QueryStringAttribute("name", name); - element->QueryStringAttribute("provider", provider); - - std::string typetag(element->Name()); - std::string nametag(*name); - std::string providertag(*provider); - - bool hasChildren = !element->NoChildren(); - bool hasSiblings = !capabilities2_xml_parser::isLastElement(element); - bool foundInControl = capabilities2_xml_parser::search(control_list, nametag); - bool foundInEvents = capabilities2_xml_parser::search(events_list, nametag); - bool foundInProviders = capabilities2_xml_parser::search(providers_list, providertag); - bool returnValue = true; - - if (typetag == "Control") - { - if (foundInControl and hasChildren) - returnValue = returnValue and capabilities2_xml_parser::check_tags(element->FirstChildElement(), events_list, providers_list, control_list); - - if (foundInControl and hasSiblings) - returnValue = returnValue and capabilities2_xml_parser::check_tags(element->NextSiblingElement(), events_list, providers_list, control_list); - - if (foundInControl and !hasSiblings) - returnValue = returnValue; - - if (!foundInControl) - return false; - } - else if (typetag == "Event") - { - if (foundInEvents and foundInProviders and hasSiblings) - returnValue = returnValue and capabilities2_xml_parser::check_tags(element->NextSiblingElement(), events_list, providers_list, control_list); - - if (foundInEvents and foundInProviders and !hasSiblings) - returnValue = returnValue; - - if (!foundInEvents) - return false; - - if (!foundInProviders) - return false; - } - else - { - return false; - } - - return returnValue; - } - - /** - * @brief Adds an event connection to the capabilities2 fabric - * - * @param source_event the source event from which the connection would start - * @param source_provider provider of the source event - * @param target_event the target event which will be connected via the connection - * @param target_provider provider of the target event - * @param connection the type of connection - * @param event_element the tinyxml2::XMLElement which contains runtime parameters - * - * @return The connection object - */ - capabilities2_executor::connection_t create_connection(std::string source_event, std::string source_provider, - std::string target_event, std::string target_provider, - capabilities2_executor::connection_type_t connection, tinyxml2::XMLElement *event_element) - { - capabilities2_executor::connection_t connect; - - connect.source_event = source_event; - connect.source_provider = source_provider; - connect.target_event = target_event; - connect.target_provider = target_provider; - connect.connection = connection; - connect.event_element = event_element; - - return connect; - } - - /** - * @brief parse through the plan and extract the connections - * - * @param element XML Element to be evaluated - * @param connection_list std::vector containing extracted connections - * @param connection the type of connection - * @param source_event source event name. if not provided, "start" is taken as default - * @param source_provider provider of the source event, if not provided "" is taken as default - * @param target_event target event name. if not provided, "stop" is taken as default - * @param target_provider provider of the target event. if not provided, "stop" is taken as default - */ - void extract_connections(tinyxml2::XMLElement *element, std::vector &connection_list, - capabilities2_executor::connection_type_t connection = capabilities2_executor::connection_type_t::ON_SUCCESS_START, - std::string source_event = "start", std::string source_provider = "", - std::string target_event = "stop", std::string target_provider = "") - { - const char **name; - const char **provider; - - element->QueryStringAttribute("name", name); - element->QueryStringAttribute("provider", provider); - - std::string typetag(element->Name()); - std::string nametag(*name); - std::string providertag(*provider); - - bool hasChildren = !element->NoChildren(); - bool hasSiblings = !capabilities2_xml_parser::isLastElement(element); - - if ((typetag == "Control") and (nametag == "sequential")) - { - if (hasChildren) - capabilities2_xml_parser::extract_connections(element->FirstChildElement(), connection_list, capabilities2_executor::connection_type_t::ON_SUCCESS_START, - source_event, source_provider, target_event, target_provider); - - if (hasSiblings) - capabilities2_xml_parser::extract_connections(element->NextSiblingElement(), connection_list, connection, - source_event, source_provider, target_event, target_provider); - } - else if ((typetag == "Control") and (nametag == "parallel")) - { - if (hasChildren) - capabilities2_xml_parser::extract_connections(element->FirstChildElement(), connection_list, capabilities2_executor::connection_type_t::ON_START_START, - source_event, source_provider, target_event, target_provider); - - if (hasSiblings) - capabilities2_xml_parser::extract_connections(element->NextSiblingElement(), connection_list, connection, - source_event, source_provider, target_event, target_provider); - } - else if ((typetag == "Control") and (nametag == "recovery")) - { - if (hasChildren) - capabilities2_xml_parser::extract_connections(element->FirstChildElement(), connection_list, capabilities2_executor::connection_type_t::ON_FAILURE_START, - source_event, source_provider, target_event, target_provider); - - if (hasSiblings) - capabilities2_xml_parser::extract_connections(element->NextSiblingElement(), connection_list, connection, - source_event, source_provider, target_event, target_provider); - } - else if (typetag == "Event") - { - capabilities2_executor::connection_t connection_obj = create_connection(source_event, source_provider, nametag, providertag, connection, element); - connection_list.push_back(connection_obj); - - if (hasSiblings) - capabilities2_xml_parser::extract_connections(element->NextSiblingElement(), connection_list, connection, - nametag, providertag, target_event, target_provider); - } - } - -} // namespace capabilities2_xml_parser diff --git a/capabilities2_executor/include/capabilities2_executor/structs/connection.hpp b/capabilities2_executor/include/capabilities2_executor/structs/connection.hpp deleted file mode 100644 index 30b8a21..0000000 --- a/capabilities2_executor/include/capabilities2_executor/structs/connection.hpp +++ /dev/null @@ -1,27 +0,0 @@ -#include -#include - -namespace capabilities2_executor -{ - enum connection_type_t - { - ON_START_START, - ON_START_STOP, - ON_SUCCESS_START, - ON_SUCCESS_STOP, - ON_FAILURE_START, - ON_FAILURE_STOP, - ON_TERMINATE_START, - ON_TERMINATE_STOP - }; - - struct connection_t { - std::string source_event; - std::string source_provider; - std::string target_event; - std::string target_provider; - connection_type_t connection; - tinyxml2::XMLElement* event_element; - }; - -} // namespace capabilities2_executor diff --git a/capabilities2_executor/launch/.gitkeep b/capabilities2_executor/launch/.gitkeep deleted file mode 100644 index e69de29..0000000 diff --git a/capabilities2_executor/package.xml b/capabilities2_executor/package.xml deleted file mode 100644 index d8d8132..0000000 --- a/capabilities2_executor/package.xml +++ /dev/null @@ -1,26 +0,0 @@ - - - - capabilities2_executor - 0.0.0 - TODO: Package description - kalana - - MIT - - ament_cmake - - rclcpp - capabilities2_msgs - rclcpp_action - - - tinyxml2 - - ament_lint_auto - ament_lint_common - - - ament_cmake - - diff --git a/capabilities2_executor/readme.md b/capabilities2_executor/readme.md deleted file mode 100644 index 999186c..0000000 --- a/capabilities2_executor/readme.md +++ /dev/null @@ -1,5 +0,0 @@ -# Capabilities2 Executor - -This is a simple executor that demonstrates how to use the capabilities2 library to execute multi-skill tasks. - -## Usage diff --git a/capabilities2_executor/src/capabilities_executor.cpp b/capabilities2_executor/src/capabilities_executor.cpp deleted file mode 100644 index 98c5a43..0000000 --- a/capabilities2_executor/src/capabilities_executor.cpp +++ /dev/null @@ -1,9 +0,0 @@ -#include - -int main(int argc, char * argv[]) -{ - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - return 0; -} diff --git a/capabilities2_executor/src/capabilities_file_parser.cpp b/capabilities2_executor/src/capabilities_file_parser.cpp deleted file mode 100644 index 00c2410..0000000 --- a/capabilities2_executor/src/capabilities_file_parser.cpp +++ /dev/null @@ -1,9 +0,0 @@ -#include - -int main(int argc, char * argv[]) -{ - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - return 0; -} diff --git a/capabilities2_launch_proxy/capabilities2_launch_proxy/__init__.py b/capabilities2_launch_proxy/capabilities2_launch_proxy/__init__.py deleted file mode 100644 index e69de29..0000000 diff --git a/capabilities2_launch_proxy/capabilities2_launch_proxy/capabilities_launch_proxy.py b/capabilities2_launch_proxy/capabilities2_launch_proxy/capabilities_launch_proxy.py deleted file mode 100755 index 26a5845..0000000 --- a/capabilities2_launch_proxy/capabilities2_launch_proxy/capabilities_launch_proxy.py +++ /dev/null @@ -1,287 +0,0 @@ -''' -capabilities launch proxy server - -implements a launch server responding to messages to launch and shutdown launch files -use in conjunction with capabilities2_server to launch capabilities - -acts like ros2 launch command but from an action interface -''' - -import os -import threading -from typing import Set -from typing import Text -from typing import Optional -import rclpy -from rclpy.executors import MultiThreadedExecutor -from rclpy.executors import ExternalShutdownException -import rclpy.logging -from rclpy.node import Node -from rclpy.action import ActionServer -from rclpy.action import CancelResponse -from rclpy.action.server import ServerGoalHandle -from launch import LaunchService -from launch import LaunchContext -from launch.event import Event -from launch.events.process import ShutdownProcess -from launch.event_handler import EventHandler -from launch.event_handlers import OnProcessStart -from launch.actions import ExecuteProcess -from launch.actions import EmitEvent -from launch.actions import RegisterEventHandler -from launch.launch_description_sources import AnyLaunchDescriptionSource -# from launch.some_entities_type import SomeEntitiesType -from capabilities2_msgs.action import Launch -from capabilities2_msgs.msg import CapabilityEvent - - -class CancelLaunchGoalEvent(Event): - """ - Launch goal cancel event - - used when a launch goal is canceled - the result should be that the launch description associated with a goal - is shutdown in the running LaunchService - other goals should not be affected - """ - - name = 'launch.event.CancelLaunchGoal' - - def __init__(self, *, goal_handle: ServerGoalHandle) -> None: - """ - Create a LaunchGoalCancelEvent - """ - self.goal_handle = goal_handle - - -class CancelEventHandler(EventHandler): - """ - Cancel event handler - - handle cancel launch goal events - cancels a launch goal if it matches the goal handle passed in - """ - - def __init__(self, goal_handle: ServerGoalHandle, pids: Set[int]): - super().__init__(matcher=lambda event: isinstance(event, CancelLaunchGoalEvent)) - self.goal_handle = goal_handle - self.pids = pids - - def handle(self, event: Event, context: LaunchContext): - """ - handle event - """ - - super().handle(event, context) - - # check if event is a cancel launch goal event - if not isinstance(event, CancelLaunchGoalEvent): - return None - - # if the goals match then cancel - if event.goal_handle == self.goal_handle: - return EmitEvent(event=ShutdownProcess( - process_matcher=lambda action: action.process_details['pid'] in self.pids - )) - - return None - - -class CapabilitiesLaunchProxy(Node): - """ - Capabilities Launch proxy server - """ - - def __init__(self, node_name='capabilities_launch_proxy'): - # super class init - super().__init__(node_name) - - # active files set - self.active_launch_files: Set[Text] = set() - self.launch_set_lock = threading.Lock() - - # create launch action server - self.launch_sub = ActionServer( - self, - Launch, - '~/launch', - handle_accepted_callback=self.handle_accepted_cb, - execute_callback=self.execute_cb, - cancel_callback=self.cancel_cb - ) - - # cap event pub - self.event_pub = self.create_publisher( - CapabilityEvent, - '~/events', - 10 - ) - - # create launch service - self.launch_service = LaunchService() - - # log start - self.get_logger().info('capabilities launch proxy started') - - # service interface - def get_active_launch_files_cb(self): - """ - get active launch files - """ - return self.active_launch_files - - # action interface - def handle_accepted_cb(self, goal_handle: ServerGoalHandle) -> None: - """ - handle accepted callback - """ - # start execution and detach - threading.Thread(target=goal_handle.execute).start() - - # execute callback is main launch feature - def execute_cb(self, goal_handle: ServerGoalHandle) -> Launch.Result: - """ - launch execute callback - """ - - # check if file exists - if not os.path.isfile(goal_handle.request.launch_file_path): - # file does not exist - self.get_logger().error("file does not exist: '{}'".format( - goal_handle.request.launch_file_path)) - - # abort goal - goal_handle.abort() - return Launch.Result() - - with self.launch_set_lock: - # launch context already exists guard - if goal_handle.request.launch_file_path in self.active_launch_files: - self.get_logger().error("launch already exists for source '{}'".format( - goal_handle.request.launch_file_path)) - - goal_handle.abort() - return Launch.Result() - - # add context to active files - self.active_launch_files.add(goal_handle.request.launch_file_path) - - # get launch description from file - description = AnyLaunchDescriptionSource( - goal_handle.request.launch_file_path).get_launch_description(self.launch_service.context) - - # set up process id get event handler - description_process_ids: Set[int] = set() - - # go through all actions and collect all processes (mostly nodes) in the description - for e in description.entities: - # check the type of entity is an execute process action - if isinstance(e, ExecuteProcess): - # register event handler for process start event - # on process start store the pid - description.add_action( - RegisterEventHandler(OnProcessStart( - target_action=e, - on_start=lambda event, context: description_process_ids.add( - event.pid) - )) - ) - - # add shutdown event to launch description - # add shutdown event handler to launch description - # this uses the cancel goal event handler and cancel goal event - description.add_action(RegisterEventHandler(CancelEventHandler( - goal_handle, - description_process_ids - ))) - - # add to launch service - self.launch_service.include_launch_description(description) - - # bind request to this thread - self.get_logger().info('launching: {}'.format( - goal_handle.request.launch_file_path - )) - - # sleep rate for holding this action open - rate = self.create_rate(1.0) - - # loop while not canceled - while rclpy.ok(): - # if cancelled - if goal_handle.is_cancel_requested: - self.get_logger().warn("launch canceled: '{}'".format( - goal_handle.request.launch_file_path - )) - - # shutdown context - self.launch_service.emit_event( - CancelLaunchGoalEvent(goal_handle=goal_handle)) - - # delete context from active files - with self.launch_set_lock: - self.active_launch_files.remove( - goal_handle.request.launch_file_path) - - goal_handle.canceled() - return Launch.Result() - - # send event feedback - feedback = Launch.Feedback() - feedback.event.type = 'launched' - goal_handle.publish_feedback(feedback) - - # spin indefinitely (sleep loop this thread) - rate.sleep() - - goal_handle.succeed() - return Launch.Result() - - def cancel_cb(self, goal_handle: ServerGoalHandle) -> CancelResponse: - """ - accept all cancellations - """ - return CancelResponse.ACCEPT - - -# main function -def main(): - """ - main function - """ - - try: - # init node - rclpy.init() - - # instantiate the capabilities launch server - capabilities_launch_proxy = CapabilitiesLaunchProxy() - - # spin node in new thread - executor = MultiThreadedExecutor() - executor.add_node(capabilities_launch_proxy) - executor_thread = threading.Thread(target=executor.spin) - executor_thread.start() - - # run the launch service - capabilities_launch_proxy.get_logger().info('running LaunchService') - capabilities_launch_proxy.launch_service.run( - shutdown_when_idle=False) - - # cancel the launch services - capabilities_launch_proxy.launch_service.shutdown() - - executor.shutdown() - executor_thread.join() - capabilities_launch_proxy.destroy_node() - - # rclpy.shutdown() - - except ExternalShutdownException: - pass - - -# main -if __name__ == '__main__': - # run the proxy - main() diff --git a/capabilities2_launch_proxy/package.xml b/capabilities2_launch_proxy/package.xml deleted file mode 100644 index ab70338..0000000 --- a/capabilities2_launch_proxy/package.xml +++ /dev/null @@ -1,19 +0,0 @@ - - - - capabilities2_launch_proxy - 0.0.1 - ros2 launch proxy to support launching capabilities similar to ros1 capabilities - mik-p - - MIT - - rclpy - launch - std_msgs - capabilities2_msgs - - - ament_python - - diff --git a/capabilities2_launch_proxy/readme.md b/capabilities2_launch_proxy/readme.md deleted file mode 100644 index e749a41..0000000 --- a/capabilities2_launch_proxy/readme.md +++ /dev/null @@ -1,46 +0,0 @@ -# capabilities2_launch_proxy - -Provides an Action API proxy to the ros2 launch framework. Implements features that are only available in python since the `launch` API is not available in C++. - -Contains the essential features to implement the ros1 [`capabilities`](https://wiki.ros.org/capabilities) package launch functionality in ros2. - -Use in conjunction with `capabilities2_server` package, to provide a full capabilities server. - -## how it works - -Implements three main functions: - -| Function | Description | -| --- | --- | -| `launch` | Run a launch file requested a runtime via a network request | -| `shutdown` | shutdown a launch file that has been started | -| `events` | send capability events during operation | - -The lifecylce of these functions are typically handled by the capabilities server but could be used for other things. - -## Use without capabilities2 server - -The proxy provides an action called `~/launch`. The `goal` is a file path to launch from. The feedback provides events about the launch status. - -### Add to a launch file - -```python -from launch import LaunchDescription -from launch_ros.actions import Node - -def generate_launch_description(): - return LaunchDescription([ - # create launch proxy node - Node( - package='capabilities2_launch_proxy', - executable='capabilities_launch_proxy', - name='capabilities_launch_proxy' - ) - ]) -``` - -### Run standalone - -```bash -ros2 run capabilities2_launch_proxy capabilities_launch_proxy -``` diff --git a/capabilities2_launch_proxy/resource/capabilities2_launch_proxy b/capabilities2_launch_proxy/resource/capabilities2_launch_proxy deleted file mode 100644 index e69de29..0000000 diff --git a/capabilities2_launch_proxy/setup.cfg b/capabilities2_launch_proxy/setup.cfg deleted file mode 100644 index d3f4af5..0000000 --- a/capabilities2_launch_proxy/setup.cfg +++ /dev/null @@ -1,4 +0,0 @@ -[develop] -script_dir=$base/lib/capabilities2_launch_proxy -[install] -install_scripts=$base/lib/capabilities2_launch_proxy diff --git a/capabilities2_launch_proxy/setup.py b/capabilities2_launch_proxy/setup.py deleted file mode 100644 index 1707673..0000000 --- a/capabilities2_launch_proxy/setup.py +++ /dev/null @@ -1,26 +0,0 @@ -from setuptools import find_packages, setup - -package_name = 'capabilities2_launch_proxy' - -setup( - name=package_name, - version='0.0.1', - packages=find_packages(exclude=['test']), - data_files=[ - ('share/ament_index/resource_index/packages', - ['resource/' + package_name]), - ('share/' + package_name, ['package.xml']), - ], - install_requires=['setuptools'], - zip_safe=True, - maintainer='mik-p', - maintainer_email='mppritchard3@hotmail.com', - description='ros2 launch proxy to support launching capabilities similar to ros1 capabilities', - license='MIT', - tests_require=['pytest'], - entry_points={ - 'console_scripts': [ - 'capabilities_launch_proxy = capabilities2_launch_proxy.capabilities_launch_proxy:main', - ], - }, -) diff --git a/capabilities2_launch_proxy/test/call_use_launch_runner.py b/capabilities2_launch_proxy/test/call_use_launch_runner.py deleted file mode 100644 index f8d3f9c..0000000 --- a/capabilities2_launch_proxy/test/call_use_launch_runner.py +++ /dev/null @@ -1,94 +0,0 @@ -''' test call cap services ''' - -from bondpy import bondpy -import rclpy -from capabilities2_msgs.srv import EstablishBond -from capabilities2_msgs.srv import GetRunningCapabilities -from capabilities2_msgs.srv import UseCapability - - -def test_get_running_capabilities(n): - # get running capabilities - client = n.create_client( - GetRunningCapabilities, - '/capabilities/get_running_capabilities' - ) - - # wait for service - client.wait_for_service() - - # send request - request = GetRunningCapabilities.Request() - future = client.call_async(request) - rclpy.spin_until_future_complete(n, future) - - # print result - for s in future.result().running_capabilities: - print(s) - - -# main -if __name__ == '__main__': - - rclpy.init() - - node = rclpy.create_node('test_call_cap_srvs') - - # do tests - # test_get_interfaces_srv(node) - # test_get_semantic_interfaces_srv(node) - # test_get_providers_srv(node) - # test_get_capability_specs_srv(node) - - print("test use capability") - # get a bond - bond_cli = node.create_client( - EstablishBond, - '/capabilities/establish_bond' - ) - - # wait for service - bond_cli.wait_for_service() - - # send request - request = EstablishBond.Request() - future = bond_cli.call_async(request) - rclpy.spin_until_future_complete(node, future) - - # print result - print(future.result()) - # keep bond id - bond_id = future.result().bond_id - - # create a use capability request - use_client = node.create_client( - UseCapability, - '/capabilities/use_capability' - ) - - # wait for service - use_client.wait_for_service() - - # send request - request = UseCapability.Request() - request.capability = 'std_capabilities/empty' - request.preferred_provider = 'std_capabilities/empty' - request.bond_id = future.result().bond_id - future = use_client.call_async(request) - rclpy.spin_until_future_complete(node, future) - - # print result - print(future.result()) - - # get running capabilities - test_get_running_capabilities(node) - - # keep bond alive - bond = bondpy.Bond(node, "/capabilities/bonds", bond_id) - bond.start() - rclpy.spin(node) - - node.destroy_node() - rclpy.shutdown() - - exit(0) diff --git a/capabilities2_msgs/CMakeLists.txt b/capabilities2_msgs/CMakeLists.txt index 669eb9c..3be2079 100644 --- a/capabilities2_msgs/CMakeLists.txt +++ b/capabilities2_msgs/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 3.10) +cmake_minimum_required(VERSION 3.16) project(capabilities2_msgs) # Default to C++17 @@ -17,37 +17,41 @@ find_package(std_msgs REQUIRED) set(msg_files "msg/Capability.msg" + "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/NaturalCapability.msg" - "msg/CapabilityCommand.msg" - "msg/CapabilityResponse.msg" - "msg/CapabilityConnection.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/TriggerCapability.srv" "srv/UseCapability.srv" ) set(action_files "action/Capability.action" "action/Launch.action" - "action/Plan.action" - "action/Connections.action" ) rosidl_generate_interfaces(${PROJECT_NAME} diff --git a/capabilities2_msgs/action/Connections.action b/capabilities2_msgs/action/Connections.action deleted file mode 100644 index 80c885d..0000000 --- a/capabilities2_msgs/action/Connections.action +++ /dev/null @@ -1,14 +0,0 @@ -# action to send a list of interface connections to capabilities server -# goal -std_msgs/Header header -string bond_id -CapabilityConnection[] connections ---- -# result -std_msgs/Header header -CapabilityConnection[] failed_connections ---- -# feedback -std_msgs/Header header -int32 success_count -int32 total_count diff --git a/capabilities2_msgs/action/Launch.action b/capabilities2_msgs/action/Launch.action index d4c26d4..c6e8c95 100644 --- a/capabilities2_msgs/action/Launch.action +++ b/capabilities2_msgs/action/Launch.action @@ -8,4 +8,3 @@ std_msgs/Header header --- # feedback std_msgs/Header header -capabilities2_msgs/CapabilityEvent event diff --git a/capabilities2_msgs/action/Plan.action b/capabilities2_msgs/action/Plan.action deleted file mode 100644 index ad84fa9..0000000 --- a/capabilities2_msgs/action/Plan.action +++ /dev/null @@ -1,12 +0,0 @@ -# action to transfer a plan for execution -# goal -std_msgs/Header header -string plan ---- -# result -std_msgs/Header header -bool success -string[] failed_elements ---- -# feedback -std_msgs/Header header diff --git a/capabilities2_msgs/msg/Capability.msg b/capabilities2_msgs/msg/Capability.msg index bab67a8..8d08754 100644 --- a/capabilities2_msgs/msg/Capability.msg +++ b/capabilities2_msgs/msg/Capability.msg @@ -1,4 +1,11 @@ # Capability string capability + # Used provider string provider + +# 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 04b538b..11ca06f 100644 --- a/capabilities2_msgs/msg/CapabilityConnection.msg +++ b/capabilities2_msgs/msg/CapabilityConnection.msg @@ -1,14 +1,11 @@ -# Declare types of connections -uint8 ON_START_START = 0 -uint8 ON_START_STOP = 1 -uint8 ON_SUCCESS_START = 2 -uint8 ON_SUCCESS_STOP = 3 -uint8 ON_FAILURE_START = 4 -uint8 ON_FAILURE_STOP = 5 -uint8 ON_TERMINATE_START = 6 -uint8 ON_TERMINATE_STOP = 7 +# a connection between capabilities +# usually used via an event -Capability source -Capability target -uint8 connection_type -string target_parameters \ No newline at end of file +# the connection type through event code +capabilities2_msgs/CapabilityEventCode type + +# Capability which this connection originates +capabilities2_msgs/Capability source + +# capability targeted by this connection +capabilities2_msgs/Capability target diff --git a/capabilities2_msgs/msg/CapabilityEvent.msg b/capabilities2_msgs/msg/CapabilityEvent.msg index 1a52c94..9501444 100644 --- a/capabilities2_msgs/msg/CapabilityEvent.msg +++ b/capabilities2_msgs/msg/CapabilityEvent.msg @@ -1,16 +1,16 @@ -std_msgs/Header header -# Capability which this event pertains to -string capability -# Capability provider which this event pertains to -string provider +# the capability event message -# Event types -string LAUNCHED="launched" -string STOPPED="stopped" -string TERMINATED="terminated" -string SERVER_READY="server_ready" -# Event type -string type +# identifying name for the source that triggered this event +string trigger_id -# PID of the related process -int32 pid +# event can pass over a connection +# could represent connection between capabilities +# usually via runners +# Capabilities which this event pertains to (source and target) +capabilities2_msgs/CapabilityConnection connection + +# the event code +capabilities2_msgs/CapabilityEventCode code + +# optional event message +string description 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/CapabilityEventStamped.msg b/capabilities2_msgs/msg/CapabilityEventStamped.msg new file mode 100644 index 0000000..668e832 --- /dev/null +++ b/capabilities2_msgs/msg/CapabilityEventStamped.msg @@ -0,0 +1,5 @@ +# capability event stamped message type +std_msgs/Header header # add header + +# the event +capabilities2_msgs/CapabilityEvent event 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/package.xml b/capabilities2_msgs/package.xml index 3ac0ada..b3730fc 100644 --- a/capabilities2_msgs/package.xml +++ b/capabilities2_msgs/package.xml @@ -1,15 +1,17 @@ capabilities2_msgs - 0.1.0 + 0.2.0 This package provides message service and action formats for capabilities2 interface Michael Pritchard mik-p + Kalana Ratnayake Michael Pritchard + Kalana Ratnayake MIT diff --git a/capabilities2_msgs/readme.md b/capabilities2_msgs/readme.md index 5c872b4..ae55b71 100644 --- a/capabilities2_msgs/readme.md +++ b/capabilities2_msgs/readme.md @@ -2,7 +2,7 @@ ![ROS](https://img.shields.io/badge/Framework-ROS-informational?&logo=ROS) -This package contains [ROS2](https://index.ros.org/doc/ros2/) messages, services, and actions for the capabilities2 interface. +This package contains [ROS2](https://index.ros.org/doc/ros2/) `messages`, `services`, and `actions` for the capabilities2 framework. ## Message Types @@ -10,7 +10,10 @@ This package contains [ROS2](https://index.ros.org/doc/ros2/) messages, services | --- | --- | | `Capability.msg` | A message type for a capability of a robot. | | `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. | | `NaturalCapability.msg` | A message type for a natural capability of a robot. | @@ -35,6 +38,12 @@ This package contains [ROS2](https://index.ros.org/doc/ros2/) messages, services | `StopCapability.srv` | A service type for stopping a capability. | | `UseCapability.srv` | A service type for using a capability. | +New in 0.1.3: + +- `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. + ## Action Types | Action Type | Description | 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/Launch.srv b/capabilities2_msgs/srv/Launch.srv new file mode 100644 index 0000000..a1938ed --- /dev/null +++ b/capabilities2_msgs/srv/Launch.srv @@ -0,0 +1,6 @@ +std_msgs/Header header +string package_name +string launch_file_name +--- +std_msgs/Header header +string status \ No newline at end of file diff --git a/capabilities2_msgs/srv/TriggerCapability.srv b/capabilities2_msgs/srv/TriggerCapability.srv new file mode 100644 index 0000000..0a52ba9 --- /dev/null +++ b/capabilities2_msgs/srv/TriggerCapability.srv @@ -0,0 +1,3 @@ +string bond_id +capabilities2_msgs/Capability capability +--- diff --git a/capabilities2_runner/CMakeLists.txt b/capabilities2_runner/CMakeLists.txt index 02d553d..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,21 +15,16 @@ find_package(rclcpp REQUIRED) find_package(rclcpp_action REQUIRED) find_package(pluginlib REQUIRED) find_package(capabilities2_msgs REQUIRED) -find_package(PkgConfig) -pkg_check_modules(TINYXML2 tinyxml2 REQUIRED) +find_package(capabilities2_events REQUIRED) -include_directories(include -${TINYXML2_INCLUDE_DIRS} +include_directories( + include ) add_library(${PROJECT_NAME} SHARED - src/standard_runners.cpp - # src/launch_runner.cpp - # src/action_runner.cpp -) - -target_link_libraries(${PROJECT_NAME} - ${TINYXML2_LIBRARIES} + src/dummy_runner.cpp + src/launch_runner.cpp + src/system/system_runners.cpp ) ament_target_dependencies(${PROJECT_NAME} @@ -37,7 +32,7 @@ ament_target_dependencies(${PROJECT_NAME} rclcpp_action pluginlib capabilities2_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 4b63d53..61f7b8c 100644 --- a/capabilities2_runner/include/capabilities2_runner/action_runner.hpp +++ b/capabilities2_runner/include/capabilities2_runner/action_runner.hpp @@ -4,12 +4,16 @@ #include #include #include +#include +#include +#include +#include #include #include #include -#include +#include namespace capabilities2_runner { @@ -20,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() { } @@ -36,67 +40,32 @@ class ActionRunner : 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 action_name action name used in the yaml file, used to load specific configuration from the run_config - * @param on_started function pointer to trigger at the start of the action client in the runner - * @param on_terminated function pointer to trigger at the termination of the action client in the runner - * @param on_stopped function pointer to trigger at the termination of the action client by the server */ - virtual void init_action(rclcpp::Node::SharedPtr node, const runner_opts& run_config, const std::string& action_name, - std::function on_started = nullptr, - std::function on_terminated = nullptr, - std::function on_stopped = nullptr) + virtual void init_action(rclcpp::Node::SharedPtr node, const runner_opts& run_config, const std::string& action_name) { // initialize the runner base by storing node pointer and run config - init_base(node, run_config, on_started, on_terminated, on_stopped); + init_base(node, run_config); // create an action client action_client_ = rclcpp_action::create_client(node_, action_name); // wait for action server - RCLCPP_INFO(node_->get_logger(), "%s waiting for action: %s", run_config_.interface.c_str(), action_name.c_str()); + RCLCPP_INFO(node_->get_logger(), "waiting for action: %s", action_name.c_str()); - if (!action_client_->wait_for_action_server(std::chrono::seconds(3))) + if (!action_client_->wait_for_action_server(std::chrono::seconds(1000))) { - RCLCPP_ERROR(node_->get_logger(), "%s failed to connect to action: %s", run_config_.interface.c_str(), - action_name.c_str()); + RCLCPP_ERROR(node_->get_logger(), "failed to connect to action: %s", action_name.c_str()); throw runner_exception("failed to connect to action server"); } - // send goal options - // goal response callback - send_goal_options_.goal_response_callback = - [this](const typename rclcpp_action::ClientGoalHandle::SharedPtr& goal_handle) { - // publish event - if (goal_handle) - if (on_started_) - on_started_(run_config_.interface); - - // store goal handle to be used with stop funtion - goal_handle_ = goal_handle; - }; - - // result callback - send_goal_options_.result_callback = - [this](const typename rclcpp_action::ClientGoalHandle::WrappedResult& wrapped_result) { - if (wrapped_result.code == rclcpp_action::ResultCode::SUCCEEDED) - { - // Do something - } - else - { - // send terminated event - if (on_terminated_) - { - on_terminated_(run_config_.interface); - } - } - }; + 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 @@ -115,27 +84,24 @@ class ActionRunner : public RunnerBase { try { - std::shared_future cancel_future = - action_client_->async_cancel_goal( - goal_handle_, [this](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"); - } - - // publish event - if (on_stopped_) - { - on_stopped_(run_config_.interface); - } - }); + auto cancel_future = action_client_->async_cancel_goal( + 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"); + RCLCPP_ERROR(node_->get_logger(), "Runner cancellation failed."); + } + + // 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 // BUG: the line below does not work in jazzy build, so a workaround is used - // rclcpp::spin_until_future_complete(node_->get_node_base_interface(), cancel_future, std::chrono::seconds(2)); auto timeout = std::chrono::steady_clock::now() + std::chrono::seconds(2); while (std::chrono::steady_clock::now() < timeout) { + // Check if the cancel operation is complete if (cancel_future.wait_for(std::chrono::milliseconds(100)) == std::future_status::ready) break; } @@ -146,78 +112,91 @@ class ActionRunner : public RunnerBase throw runner_exception(e.what()); } } + + RCLCPP_INFO(node_->get_logger(), "runner cleaned. stopping.."); } +protected: /** - * @brief the trigger function on the action runner is used to trigger an action. - * this method provides a mechanism for injecting parameters or a goal into the action - * and then trigger the action + * @brief Trigger process to be executed. + * + * This method utilizes paramters set via the trigger() function * - * @param parameters - * @return std::optional)>> + * @param parameters pointer to capabilities2_events::EventParameters that contains parameters */ - virtual std::optional)>> - trigger(std::shared_ptr parameters = nullptr) override + virtual void execution(capabilities2_events::EventParameters parameters, const std::string& thread_id) override { - // the action is being triggered out of order - if (!goal_handle_) - throw runner_exception("cannot trigger action that was not started"); + // 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); - // if parameters are not provided then cannot proceed - if (!parameters) - throw runner_exception("cannot trigger action without parameters"); + // 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()); - // generate a goal from parameters if provided - typename ActionT::Goal goal_msg = generate_goal(parameters); + std::mutex block_mutex; + std::condition_variable cv; + bool completed = false; + std::unique_lock lock(block_mutex); // trigger the action client with goal - auto goal_handle_future = action_client_->async_send_goal(goal_msg, send_goal_options_); - - // spin until send future completes - if (rclcpp::spin_until_future_complete(node_, goal_handle_future) != rclcpp::FutureReturnCode::SUCCESS) - { - RCLCPP_ERROR(node_->get_logger(), "send goal call failed"); - return std::nullopt; - } - - // get result future - typename rclcpp_action::ClientGoalHandle::SharedPtr goal_handle; - auto result_future = action_client_->async_get_result(goal_handle); - - // create a function to call for the result - // the future will be returned to the caller - // and the caller can provide a conversion function - // to handle the result - std::function)> result_callback = - [this, result_future](std::shared_ptr result) { - // wait for result - if (rclcpp::spin_until_future_complete(node_, result_future) != rclcpp::FutureReturnCode::SUCCESS) + send_goal_options_.goal_response_callback = + [this, &instance_id](const typename rclcpp_action::ClientGoalHandle::SharedPtr& goal_handle) { + if (goal_handle) { - RCLCPP_ERROR(node_->get_logger(), "get result call failed"); - return; + RCLCPP_INFO(node_->get_logger(), "goal accepted. Waiting for result for instance %s", instance_id.c_str()); } + else + { + 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, &instance_id]( + typename rclcpp_action::ClientGoalHandle::SharedPtr goal_handle, + const typename ActionT::Feedback::ConstSharedPtr feedback_msg) { + std::string feedback = generate_feedback(feedback_msg); - // get wrapped result - typename rclcpp_action::ClientGoalHandle::WrappedResult wrapped_result = result_future.get(); + if (feedback != "") + { + RCLCPP_INFO(node_->get_logger(), "received feedback: %s for instance %s", feedback.c_str(), instance_id.c_str()); + } + }; - // convert the result + send_goal_options_.result_callback = + [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) { - // publish event - if (on_result_) - { - on_result_(run_config_.interface); - } - - result = generate_result(wrapped_result.result); - return; + 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 + { + 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(); }; - return result_callback; + goal_handle_future_ = action_client_->async_send_goal(goal_msg_, send_goal_options_); + RCLCPP_INFO(node_->get_logger(), "goal sent. Waiting for acceptance for instance %s", instance_id.c_str()); + + // Conditional wait + 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 * @@ -226,24 +205,25 @@ 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(std::shared_ptr parameters) = 0; + virtual typename ActionT::Goal generate_goal(capabilities2_events::EventParameters& parameters) = 0; /** - * @brief generate a typed erased goal result + * @brief Generate a std::string from feedback message * - * this method is used in a callback passed to the trigger caller to get type erased result - * from the action the result can be passed by the caller or ignored + * This function is used to convert feedback messages into generic strings * - * The pattern needs to be implemented in the derived class + * A pattern needs to be implemented in the derived class. If the feedback string + * is empty, nothing will be printed on the screen * - * @param wrapped_result - * @return std::shared_ptr + * @param msg the feedback message received from the action server + * @return ActionT::Feedback the received feedback */ - virtual std::shared_ptr generate_result(const typename ActionT::Result::SharedPtr& result) = 0; + virtual std::string generate_feedback(const typename ActionT::Feedback::ConstSharedPtr msg) = 0; +protected: /**< action client */ typename rclcpp_action::Client::SharedPtr action_client_; @@ -253,6 +233,21 @@ class ActionRunner : public RunnerBase /** goal handle parameter to capture goal response from goal_response_callback */ typename rclcpp_action::ClientGoalHandle::SharedPtr goal_handle_; + + /** Wrapped Result */ + typename rclcpp_action::ClientGoalHandle::WrappedResult wrapped_result_; + + /** Result */ + typename ActionT::Result::SharedPtr result_; + + /** Goal message */ + typename ActionT::Goal goal_msg_; + + /** Goal Handle Future message */ + std::shared_future::SharedPtr> goal_handle_future_; + + /** Result Future*/ + std::shared_future::WrappedResult> result_future_; }; } // namespace capabilities2_runner diff --git a/capabilities2_runner/include/capabilities2_runner/encap_runner.hpp b/capabilities2_runner/include/capabilities2_runner/encap_runner.hpp index 0e45708..4f059a6 100644 --- a/capabilities2_runner/include/capabilities2_runner/encap_runner.hpp +++ b/capabilities2_runner/include/capabilities2_runner/encap_runner.hpp @@ -33,21 +33,15 @@ class EnCapRunner : public NoTriggerActionRunner * this is deferred since the action client topic name is not known at this level * of abstraction * - * @param node - * @param run_config - * @param action_name - * @param on_started - * @param on_terminated - * @param on_stopped + * @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 action_name action name used in the yaml file, used to load specific configuration from the run_config */ virtual void init_encapsulated_action(rclcpp::Node::SharedPtr node, const runner_opts& run_config, - const std::string& action_name, - std::function on_started = nullptr, - std::function on_terminated = nullptr, - std::function on_stopped = nullptr) + const std::string& action_name, std::function print) { // init the base action runner - init_action(node, run_config, action_name, on_started, on_terminated, on_stopped); + init_action(node, run_config, action_name, print); // create an encapsulating action server encap_action_ = rclcpp_action::create_server( @@ -63,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 @@ -99,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 9323d9a..68a8166 100644 --- a/capabilities2_runner/include/capabilities2_runner/launch_runner.hpp +++ b/capabilities2_runner/include/capabilities2_runner/launch_runner.hpp @@ -1,10 +1,6 @@ #pragma once -#include -#include -#include -#include -#include +#include namespace capabilities2_runner { @@ -13,55 +9,61 @@ 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 NoTriggerActionRunner +class LaunchRunner : public NoTriggerRunner { public: - LaunchRunner() : NoTriggerActionRunner() + /** + * @brief Constructor which needs to be empty due to plugin semantics + */ + LaunchRunner() : NoTriggerRunner() { } - virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config, - std::function on_started = nullptr, - std::function on_terminated = nullptr, - std::function on_stopped = nullptr) override + /** + * @brief Starter function for starting the launch 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 bond identifier for the runner + */ + virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config, const std::string& bond_id) override { - // store node pointer and run_config - init_action(node, run_config, "capabilities_launch_proxy/launch", on_started, on_terminated, on_stopped); + init_base(node, run_config); - // get the package path from environment variable - std::string package_path; - try - { - package_path = ament_index_cpp::get_package_share_directory(get_package_name()); - } - catch (const std::exception& e) - { - RCLCPP_ERROR(node_->get_logger(), "Failed to get package share directory: %s", e.what()); - throw runner_exception("failed to get package share directory"); - } + package_name = run_config_.runner.substr(0, run_config_.runner.find("/")); + launch_name = run_config_.runner.substr(run_config_.runner.find("/") + 1); - // resolve launch path - // get full path to launch file - // join package path with package name using path functions - std::string launch_file_path = std::filesystem::path(package_path).append(run_config_.runner).string(); + // start launch process + throw runner_exception("launch runner not implemented yet"); - // the launch file path - RCLCPP_DEBUG(node_->get_logger(), "launch file path: %s", launch_file_path.c_str()); + // 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 - // create a launch goal - capabilities2_msgs::action::Launch::Goal goal; - goal.launch_file_path = launch_file_path; + if (!node_) + throw runner_exception("cannot stop runner that was not started"); - send_goal_options_.result_callback = nullptr; + // stop the launch process + throw runner_exception("launch runner not implemented yet"); - // launch runner using action client - action_client_->async_send_goal(goal, send_goal_options_); + // emit stopped event + emit_stopped(bond_id, instance_id, param_on_stopped()); } -private: - /** launch file path */ - std::string launch_file_path; +protected: + std::string launch_name; + std::string package_name; }; } // namespace capabilities2_runner diff --git a/capabilities2_runner/include/capabilities2_runner/multi_action_runner.hpp b/capabilities2_runner/include/capabilities2_runner/multi_action_runner.hpp deleted file mode 100644 index 1cbb1c1..0000000 --- a/capabilities2_runner/include/capabilities2_runner/multi_action_runner.hpp +++ /dev/null @@ -1,266 +0,0 @@ -#pragma once - -#include -#include -#include -#include -#include - -#include -#include -#include - -#include - -namespace capabilities2_runner -{ - -/** - * @brief templated struct to handle Action clients and their respective goal_handles - * - * @tparam ActionT action type - */ -template -struct ActionClientBundle -{ - std::shared_ptr> action_client; - typename rclcpp_action::ClientGoalHandle::SharedPtr goal_handle; - typename rclcpp_action::Client::SendGoalOptions send_goal_options; -}; - -/** - * @brief Multi action runner base class - * - * Create action clients to run multiple action based capabilities - * this class provides helpers to set up sequential and concurrent actions - */ -class MultiActionRunner : public RunnerBase -{ -public: - /** - * @brief Constructor which needs to be empty due to plugin semantics - */ - MultiActionRunner() : RunnerBase() - { - } - - /** - * @brief create action clients - * - * Create a new action client and store it in the map - * - * @param action_name action name used in the yaml file, used to load specific configuration from the run_config - */ - template - void init_action(const std::string& action_name) - { - typename rclcpp_action::ClientGoalHandle::SharedPtr goal_handle_; - typename rclcpp_action::Client::SendGoalOptions send_goal_options_; - - auto client_ = rclcpp_action::create_client(node_, action_name); - - // wait for action server - RCLCPP_INFO(node_->get_logger(), "%s waiting for action: %s", run_config_.interface.c_str(), action_name.c_str()); - - if (!client_->wait_for_action_server(std::chrono::seconds(3))) - { - RCLCPP_ERROR(node_->get_logger(), "%s failed to connect to action: %s", run_config_.interface.c_str(), - action_name.c_str()); - throw runner_exception("failed to connect to action server"); - } - - ActionClientBundle bundle{ client_, goal_handle_, send_goal_options_ }; - - action_clients_map_[action_name] = std::make_any>(bundle); - } - - /** - * @brief Deinitializer function for stopping an the action - * - * Stop the named action and delete the action client - * - * @param action_name action name used in the yaml file, used to load specific configuration from the run_config - */ - template - void deinit_action(const std::string& action_name) - { - auto bundle = std::any_cast>(action_clients_map_[action_name]); - - // 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 action client is null - // this can happen if the runner is not able to find the action resource - if (!bundle.action_client) - throw runner_exception("cannot stop runner action that was not started"); - - // stop runner using action client - if (bundle.goal_handle) - { - try - { - auto cancel_future = bundle.action_client->async_cancel_goal( - bundle.goal_handle, [this](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"); - } - - // publish event - if (on_stopped_) - { - on_stopped_(run_config_.interface); - } - }); - - // wait for action to be stopped. hold the thread for 2 seconds to help keep callbacks in scope - rclcpp::spin_until_future_complete(node_, cancel_future, std::chrono::seconds(2)); - } - catch (const rclcpp_action::exceptions::UnknownGoalHandleError& e) - { - RCLCPP_ERROR(node_->get_logger(), "failed to cancel goal: %s", e.what()); - throw runner_exception(e.what()); - } - } - - // delete action client - bundle.action_client.reset(); - action_clients_map_.erase(action_name); - } - - /** - * @brief Trigger function for calling and triggering an action. Non blocking implementation so result will not - * be returned. Use when action triggering is required and result message of the action is not required. - * - * @param action_name action name used in the yaml file, used to load specific configuration from the run_config - * @param goal_msg goal message to be sent to the action server - * - * @returns True for success of launching an action. False for failure to launching the action. - */ - template - bool trigger_action(const std::string& action_name, typename ActionT::Goal& goal_msg) - { - auto bundle = std::any_cast>(action_clients_map_[action_name]); - - // result callback - bundle.send_goal_options.result_callback = - [this](const typename rclcpp_action::ClientGoalHandle::WrappedResult& wrapped_result) { - if (wrapped_result.code != rclcpp_action::ResultCode::SUCCEEDED) - { - // send terminated event - if (on_terminated_) - { - on_terminated_(run_config_.interface); - } - } - }; - - auto goal_handle_future = bundle.action_client->async_send_goal(goal_msg, bundle.send_goal_options); - - if (rclcpp::spin_until_future_complete(node_, goal_handle_future) != rclcpp::FutureReturnCode::SUCCESS) - { - RCLCPP_ERROR(node_->get_logger(), "send goal call failed :("); - return false; - } - - typename rclcpp_action::ClientGoalHandle::SharedPtr goal_handle = goal_handle_future.get(); - - if (!goal_handle) - { - RCLCPP_ERROR(node_->get_logger(), "Goal was rejected by server"); - return false; - } - else - { - // publish event - if (on_started_) - on_started_(run_config_.interface); - - // store goal handle to be used with stop funtion - bundle.goal_handle = goal_handle; - return true; - } - - action_clients_map_[action_name] = std::make_any>(bundle); - } - - /** - * @brief Trigger function for calling and triggering an action. Blocking implementation so result will be returned. - * Use when result message of the action is required. - * - * @param action_name action name used in the yaml file, used to load specific configuration from the run_config - * @param goal_msg goal message to be sent to the action server - * @param result_msg result message returned by the action server upon completion - * - * @returns True for success of completing an action. False for failure to complete the action. - */ - template - bool trigger_action_wait(const std::string& action_name, typename ActionT::Goal& goal_msg, - typename ActionT::Result::SharedPtr result_msg) - { - auto bundle = std::any_cast>(action_clients_map_[action_name]); - - auto goal_handle_future = bundle.action_client->async_send_goal(goal_msg, bundle.send_goal_options); - - if (rclcpp::spin_until_future_complete(node_, goal_handle_future) != rclcpp::FutureReturnCode::SUCCESS) - { - RCLCPP_ERROR(node_->get_logger(), "send goal call failed :("); - return false; - } - - typename rclcpp_action::ClientGoalHandle::SharedPtr goal_handle = goal_handle_future.get(); - - if (!goal_handle) - { - RCLCPP_ERROR(node_->get_logger(), "Goal was rejected by server"); - return false; - } - else - { - // publish event - if (on_started_) - on_started_(run_config_.interface); - - // store goal handle to be used with stop funtion - bundle.goal_handle = goal_handle; - } - - // Wait for the server to be done with the goal - auto result_future = bundle.action_client->async_get_result(goal_handle); - - RCLCPP_INFO(node_->get_logger(), "Waiting for result"); - - if (rclcpp::spin_until_future_complete(node_, result_future) != rclcpp::FutureReturnCode::SUCCESS) - { - RCLCPP_ERROR(node_->get_logger(), "get result call failed :("); - return false; - } - - typename rclcpp_action::ClientGoalHandle::WrappedResult wrapped_result = result_future.get(); - - if (wrapped_result.code == rclcpp_action::ResultCode::SUCCEEDED) - { - result_msg = wrapped_result.result; - return true; - } - else - { - // send terminated event - if (on_terminated_) - on_terminated_(run_config_.interface); - return false; - } - } - -protected: - /** - * Dictionary to hold action client bundle. The key is a string, and the value is a - * polymorphic bundle. - * */ - std::map action_clients_map_; -}; - -} // namespace capabilities2_runner diff --git a/capabilities2_runner/include/capabilities2_runner/multiplex_base_runner.hpp b/capabilities2_runner/include/capabilities2_runner/multiplex_base_runner.hpp new file mode 100644 index 0000000..10d2c61 --- /dev/null +++ b/capabilities2_runner/include/capabilities2_runner/multiplex_base_runner.hpp @@ -0,0 +1,93 @@ +#pragma once + +#include +#include +#include +#include + +namespace capabilities2_runner +{ +class MultiplexBaseRunner : public RunnerBase +{ +public: + /** + * @brief Constructor which needs to be empty due to plugin semantics + */ + MultiplexBaseRunner() : RunnerBase() + { + } + + /** + * @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) override + { + init_base(node, run_config); + } + + /** + * @brief Trigger process to be executed. + * + * @param id unique identifier for the execution + */ + virtual void execution(int id) + { + info_("execution started for id: " + std::to_string(id)); + + // check if the id is already completed + if (completed_executions.find(id) != completed_executions.end() && completed_executions[id]) + { + info_("execution already completed for id: " + std::to_string(id)); + return; + } + else + { + // trigger the events related to on_success state + if (events[id].on_success.interface != "") + { + event_(EventType::SUCCEEDED, id, events[id].on_success.interface, events[id].on_success.provider); + triggerFunction_(events[id].on_success.interface, update_on_success(events[id].on_success.parameters)); + } + // trigger the events related to on_failure state + else if (events[id].on_failure.interface != "") + { + event_(EventType::FAILED, id, events[id].on_failure.interface, events[id].on_failure.provider); + triggerFunction_(events[id].on_failure.interface, update_on_failure(events[id].on_failure.parameters)); + } + } + + // track the execution as completed + completed_executions[id] = true; + + info_("multiplexing complete. Thread closing.", id); + } + + /** + * @brief stop function to cease functionality and shutdown + * + */ + virtual void stop() override + { + // if the node pointer is empty then throw an error + // this means that the runner was not started and is being used out of order + + if (!node_) + throw runner_exception("cannot stop runner that was not started"); + + info_("stopping runner"); + } + +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 \ No newline at end of file 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 edc07eb..0000000 --- a/capabilities2_runner/include/capabilities2_runner/notrigger_action_runner.hpp +++ /dev/null @@ -1,40 +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 - std::optional)>> - trigger(std::shared_ptr parameters) override - { - throw runner_exception("cannot trigger this is a no-trigger action runner"); - } - -protected: - // throw on xml conversion functions - typename ActionT::Goal generate_goal(std::shared_ptr) override - { - throw runner_exception("cannot generate goal this is a no-trigger action runner"); - } - - std::shared_ptr generate_result(const typename ActionT::Result::SharedPtr& result) override - { - throw runner_exception("cannot generate result 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 1c12a84..2aefd08 100644 --- a/capabilities2_runner/include/capabilities2_runner/runner_base.hpp +++ b/capabilities2_runner/include/capabilities2_runner/runner_base.hpp @@ -3,10 +3,15 @@ #include #include #include -#include -#include + #include +#include +#include + +#include +#include + namespace capabilities2_runner { /** @@ -56,12 +61,19 @@ struct runner_opts std::string runner; std::string started_by; std::string pid; + 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: - RunnerBase() : run_config_() + RunnerBase() : node_(nullptr), run_config_() { } @@ -76,54 +88,86 @@ 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 on_started pointer to function to execute on starting the runner - * @param on_terminated pointer to function to execute on terminating the runner - * @param on_stopped pointer to function to execute on stopping the runner + * + * @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, - std::function on_started = nullptr, - std::function on_terminated = nullptr, - std::function on_stopped = nullptr) = 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; /** - * @brief trigger the runner + * 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 + * 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. * - * @return std::optional)>> function pointer to invoke - * elsewhere such as an event callback */ - virtual std::optional)>> - trigger(std::shared_ptr parameters = nullptr) = 0; + 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 * * @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 on_started pointer to function to execute on starting the runner - * @param on_terminated pointer to function to execute on terminating the runner */ - void init_base(rclcpp::Node::SharedPtr node, const runner_opts& run_config, - std::function on_started = nullptr, - std::function on_terminated = nullptr, - std::function on_stopped = nullptr) + 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; - on_started_ = on_started; - on_terminated_ = on_terminated; - on_stopped_ = on_stopped; + } + + /** + * @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 make EventNode::add_connection public method on runner api. + * + * add connection to this runner to target capability this allows the runner to emit events on state changes + * to the target capability the connection ID format is: "bond_id/trigger_id" which allows event emission to + * extract bond_id for access control + * + * @param connection_id unique identifier for the connection (format: "bond_id/trigger_id") + * @param type type of event to connect to + * @param target target capability to connect to + * @param callback callback to trigger target capability with (capability, parameters, bond_id) + */ + void add_connection(const std::string& connection_id, const capabilities2_msgs::msg::CapabilityEventCode& type, + const capabilities2_msgs::msg::Capability& target, + std::function + callback) + { + EventNode::add_connection(connection_id, type, target, callback); } /** @@ -167,6 +211,68 @@ class RunnerBase } protected: + // FIXME: implement new event subsystem + + /** + * @brief Update on_started event parameters with new data if available. + * + * 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 + * + * @return CapabilityOptions containing new parameters + */ + virtual capabilities2_events::EventParameters param_on_started() + { + return capabilities2_events::EventParameters(); + }; + + /** + * @brief Update on_stopped event parameters with new data if available. + * + * 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 + * + * @return CapabilityOptions containing new parameters + */ + virtual capabilities2_events::EventParameters param_on_stopped() + { + return capabilities2_events::EventParameters(); + }; + + /** + * @brief Update on_failure event parameters with new data if available. + * + * 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 + * + * @return CapabilityOptions containing new parameters + */ + virtual capabilities2_events::EventParameters param_on_failure() + { + return capabilities2_events::EventParameters(); + }; + + /** + * @brief Update on_success event parameters with new data if available. + * + * 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 + * + * @return CapabilityOptions containing new parameters + */ + virtual capabilities2_events::EventParameters param_on_success() + { + return capabilities2_events::EventParameters(); + }; + // run config getters /** @@ -300,36 +406,72 @@ class RunnerBase return get_first_resource_name("action"); } -protected: + // FIXME: implement new event subsystem + // STATE CHANGE HELPERS + /** - * @param node shared pointer to the capabilities node. Allows to use ros node related functionalities + * @brief emit STARTED event + * + * @param bond_id + * @param parameters */ - rclcpp::Node::SharedPtr node_; + void emit_started(const std::string& bond_id, const std::string& instance_id, + capabilities2_events::EventParameters parameters = capabilities2_events::EventParameters()) + { + 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); + } /** - * @param node runner configuration + * @brief emit STOPPED event + * + * @param bond_id + * @param parameters */ - runner_opts run_config_; + void emit_stopped(const std::string& bond_id, const std::string& instance_id, + capabilities2_events::EventParameters parameters = capabilities2_events::EventParameters()) + { + 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); + } /** - * @param node pointer to function to execute on starting the runner + * @brief emit SUCCEEDED event + * + * @param bond_id + * @param parameters */ - std::function on_started_; + void emit_succeeded(const std::string& bond_id, const std::string& instance_id, + capabilities2_events::EventParameters parameters = capabilities2_events::EventParameters()) + { + 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); + } /** - * @param node pointer to function to execute on terminating the runner + * @brief emit FAILED event + * + * @param bond_id + * @param parameters */ - std::function on_terminated_; + void emit_failed(const std::string& bond_id, const std::string& instance_id, + capabilities2_events::EventParameters parameters = capabilities2_events::EventParameters()) + { + 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: /** - * @param node pointer to function to execute on stopping the runner + * @brief shared pointer to the capabilities node + * Allows to use ros node related functionalities */ - std::function on_stopped_; + rclcpp::Node::SharedPtr node_; /** - * @param node pointer to function to execute on result + * @brief run_config_ runner configuration */ - std::function on_result_; + runner_opts run_config_; }; } // namespace capabilities2_runner diff --git a/capabilities2_runner/include/capabilities2_runner/service_runner.hpp b/capabilities2_runner/include/capabilities2_runner/service_runner.hpp new file mode 100644 index 0000000..2a9e9c9 --- /dev/null +++ b/capabilities2_runner/include/capabilities2_runner/service_runner.hpp @@ -0,0 +1,161 @@ +#pragma once + +#include + +#include + +namespace capabilities2_runner +{ + +/** + * @brief service runner base class + * + * Create an server client to run an service based capability + */ +template +class ServiceRunner : public ThreadTriggerRunner +{ +public: + /** + * @brief Constructor which needs to be empty due to plugin semantics + */ + ServiceRunner() : ThreadTriggerRunner() + { + } + + /** + * @brief Initializer function for initializing the service runner in place of constructor due to plugin semantics + * + * @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 service_name action name used in the yaml file, used to load specific configuration from the run_config + */ + virtual void init_service(rclcpp::Node::SharedPtr node, const runner_opts& run_config, + const std::string& service_name) + { + // initialize the runner base by storing node pointer and run config + init_base(node, run_config); + + // create a service client + service_client_ = node_->create_client(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))) + { + RCLCPP_ERROR(node_->get_logger(), "failed to connect to service: %s", service_name.c_str()); + throw runner_exception("failed to connect to server"); + } + + 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(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); + + // generate a goal from parameters if provided + auto request_msg = std::make_shared(generate_request(parameters)); + + RCLCPP_INFO(node_->get_logger(), "request generated for event :%s", instance_id.c_str()); + + 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, &instance_id, &completed, &bond_id, &cv](typename rclcpp::Client::SharedFuture future) { + if (!future.valid()) + { + RCLCPP_ERROR(node_->get_logger(), "get result call failed"); + + // emit failed event + emit_failed(bond_id, instance_id, param_on_failure()); + } + else + { + RCLCPP_INFO(node_->get_logger(), "get result call succeeded for event :%s", instance_id.c_str()); + + response_ = future.get(); + process_response(response_); + + // emit success event + emit_succeeded(bond_id, instance_id, param_on_success()); + } + + completed = true; + cv.notify_all(); + }); + + // Conditional wait + cv.wait(lock, [&completed] { return completed; }); + RCLCPP_INFO(node_->get_logger(), "Service request complete. Thread closing."); + } + +protected: + /** + * @brief Generate a request from parameters + * + * This function is used in conjunction with the trigger function to inject type erased parameters + * into the typed service + * + * A pattern needs to be implemented in the derived class + * + * @param parameters + * @return ServiceT::Request the generated request + */ + virtual typename ServiceT::Request generate_request(capabilities2_events::EventParameters& parameters) = 0; + + /** + * @brief Process the reponse and print data as required + * + * @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*/) {} + + typename rclcpp::Client::SharedPtr service_client_; + typename ServiceT::Response::SharedPtr response_; +}; +} // 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 new file mode 100644 index 0000000..795279e --- /dev/null +++ b/capabilities2_runner/include/capabilities2_runner/topic_runner.hpp @@ -0,0 +1,146 @@ +#pragma once + +#include + +#include + +namespace capabilities2_runner +{ + +/** + * @brief Topic runner base class + * + * Create an topic subsriber for data grabbing capability + */ +template +class TopicRunner : public ThreadTriggerRunner +{ +public: + /** + * @brief Constructor which needs to be empty due to plugin semantics + */ + TopicRunner() : ThreadTriggerRunner() + { + } + + /** + * @brief Initializer function for initializing the topic runner in place of constructor due to plugin semantics + * + * @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 topic_name topic name used in the yaml file, used to load specific configuration from the run_config + */ + virtual void init_subscriber(rclcpp::Node::SharedPtr node, const runner_opts& run_config, + const std::string& topic_name) + { + // initialize the runner base by storing node pointer and run config + init_base(node, run_config); + + // create an service client + subscription_ = node_->create_subscription( + 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(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); + + // emit started event + emit_started(bond_id, instance_id, param_on_started()); + + // block until a message is received in the callback and stored in latest_message_ + std::unique_lock lock(block_mutex_); + completed_ = false; + + RCLCPP_INFO(node_->get_logger(), "Waiting for Message."); + + // Conditional wait + cv_.wait(lock, [this] { return completed_; }); + RCLCPP_INFO(node_->get_logger(), "Message Received."); + + if (latest_message_) + { + // emit success event + emit_succeeded(bond_id, instance_id, param_on_success()); + } + else + { + RCLCPP_ERROR(node_->get_logger(), "Message receiving failed."); + // emit failed event + emit_failed(bond_id, instance_id, param_on_failure()); + } + + RCLCPP_INFO(node_->get_logger(), "Thread closing."); + } + +protected: + /** + * @brief Callback to be executed when receiving a message + * + * This function is used grab the messages from the callback queue into a class + * parameter so that it can be used later on dering trigger + * + * @param msg message parameter + */ + void callback(const typename TopicT::SharedPtr& msg) + { + latest_message_ = msg; + + completed_ = true; + 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 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 e010e22..4fc4f5a 100644 --- a/capabilities2_runner/package.xml +++ b/capabilities2_runner/package.xml @@ -1,15 +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 @@ -20,12 +21,29 @@ pluginlib std_msgs capabilities2_msgs + capabilities2_events - - uuid - tinyxml2 + 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 69f3745..0000000 --- a/capabilities2_runner/src/standard_runners.cpp +++ /dev/null @@ -1,46 +0,0 @@ -#include -#include -#include - -namespace capabilities2_runner -{ -class DummyRunner : public RunnerBase -{ -public: - void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config, - std::function on_started = nullptr, - std::function on_terminated = nullptr, - std::function on_stopped = nullptr) override - { - // init the base runner - init_base(node, run_config, on_started, on_terminated, on_stopped); - - // 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"); - } - - std::optional)>> - trigger(std::shared_ptr parameters) override - { - RCLCPP_INFO(node_->get_logger(), "Dummy runner cannot trigger"); - return std::nullopt; - } -}; - -} // 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_runner_audio/.clang-format b/capabilities2_runner_audio/.clang-format deleted file mode 100644 index d36804f..0000000 --- a/capabilities2_runner_audio/.clang-format +++ /dev/null @@ -1,64 +0,0 @@ - -BasedOnStyle: Google -AccessModifierOffset: -2 -ConstructorInitializerIndentWidth: 2 -AlignEscapedNewlinesLeft: false -AlignTrailingComments: true -AllowAllParametersOfDeclarationOnNextLine: false -AllowShortIfStatementsOnASingleLine: false -AllowShortLoopsOnASingleLine: false -AllowShortFunctionsOnASingleLine: None -AlwaysBreakTemplateDeclarations: true -AlwaysBreakBeforeMultilineStrings: false -BreakBeforeBinaryOperators: false -BreakBeforeTernaryOperators: false -BreakConstructorInitializersBeforeComma: true -BinPackParameters: true -ColumnLimit: 120 -ConstructorInitializerAllOnOneLineOrOnePerLine: true -DerivePointerBinding: false -PointerBindsToType: true -ExperimentalAutoDetectBinPacking: false -IndentCaseLabels: true -MaxEmptyLinesToKeep: 1 -NamespaceIndentation: None -ObjCSpaceBeforeProtocolList: true -PenaltyBreakBeforeFirstCallParameter: 19 -PenaltyBreakComment: 60 -PenaltyBreakString: 1 -PenaltyBreakFirstLessLess: 1000 -PenaltyExcessCharacter: 1000 -PenaltyReturnTypeOnItsOwnLine: 90 -SpacesBeforeTrailingComments: 2 -Cpp11BracedListStyle: false -Standard: Auto -IndentWidth: 2 -TabWidth: 2 -UseTab: Never -IndentFunctionDeclarationAfterType: false -SpacesInParentheses: false -SpacesInAngles: false -SpaceInEmptyParentheses: false -SpacesInCStyleCastParentheses: false -SpaceAfterControlStatementKeyword: true -SpaceBeforeAssignmentOperators: true -ContinuationIndentWidth: 4 -SortIncludes: false -SpaceAfterCStyleCast: false - -# Configure each individual brace in BraceWrapping -BreakBeforeBraces: Custom - -# Control of individual brace wrapping cases -BraceWrapping: { - AfterClass: 'true' - AfterControlStatement: 'true' - AfterEnum : 'true' - AfterFunction : 'true' - AfterNamespace : 'true' - AfterStruct : 'true' - AfterUnion : 'true' - BeforeCatch : 'true' - BeforeElse : 'true' - IndentBraces : 'false' -} diff --git a/capabilities2_runner_audio/include/capabilities2_runner_audio/listen_runner.hpp b/capabilities2_runner_audio/include/capabilities2_runner_audio/listen_runner.hpp deleted file mode 100644 index 0df18c1..0000000 --- a/capabilities2_runner_audio/include/capabilities2_runner_audio/listen_runner.hpp +++ /dev/null @@ -1,73 +0,0 @@ -#pragma once - -#include - -#include -#include -#include - -#include -#include - -namespace capabilities2_runner -{ - -/** - * @brief action runner base class - * - * Class to run VoiceListener action based capability - * - */ -class VoiceListenerRunner : public MultiActionRunner -{ -public: - VoiceListenerRunner() : MultiActionRunner() - { - } - - /** - * @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 on_started function pointer to trigger at the start of the action client in the runner - * @param on_terminated function pointer to trigger at the termination of the action client in the runner - */ - virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config, - std::function on_started = nullptr, - std::function on_terminated = nullptr, - std::function on_stopped = nullptr) override - { - init_base(node, run_config, on_started, on_terminated, on_stopped); - - init_action("speech_to_text"); - } - - /** - * @brief stop function to cease functionality and shutdown - * - */ - virtual void stop() override - { - deinit_action("speech_to_text"); - } - - virtual std::optional)>> - trigger(std::shared_ptr parameters = nullptr) override - { - hri_audio_msgs::action::SpeechToText::Goal goal_msg; - hri_audio_msgs::action::SpeechToText::Result::SharedPtr result_msg; - - goal_msg.header.stamp = node_->get_clock()->now(); - - return [this, &goal_msg, &result_msg](std::shared_ptr result) { - bool action_result = - trigger_action_wait("speech_to_text", goal_msg, result_msg); - result->BoolAttribute("result", action_result); - }; - } - -protected: -}; - -} // namespace capabilities2_runner diff --git a/capabilities2_runner_audio/include/capabilities2_runner_audio/speak_runner.hpp b/capabilities2_runner_audio/include/capabilities2_runner_audio/speak_runner.hpp deleted file mode 100644 index e69de29..0000000 diff --git a/capabilities2_runner_audio/package.xml b/capabilities2_runner_audio/package.xml deleted file mode 100644 index 915904c..0000000 --- a/capabilities2_runner_audio/package.xml +++ /dev/null @@ -1,30 +0,0 @@ - - - - capabilities2_runner_audio - 0.0.0 - TODO: Package description - kalana - - MIT - - ament_cmake - - rclcpp - rclcpp_action - pluginlib - std_msgs - hri_audio_msgs - capabilities2_msgs - capabilities2_runner - - - tinyxml2 - - ament_lint_auto - ament_lint_common - - - ament_cmake - - diff --git a/capabilities2_runner_audio/plugins.xml b/capabilities2_runner_audio/plugins.xml deleted file mode 100644 index d43e9c3..0000000 --- a/capabilities2_runner_audio/plugins.xml +++ /dev/null @@ -1,7 +0,0 @@ - - - - A plugin that provide speech to text capability - - - \ No newline at end of file diff --git a/capabilities2_runner_audio/src/audio_runners.cpp b/capabilities2_runner_audio/src/audio_runners.cpp deleted file mode 100644 index f7b73c5..0000000 --- a/capabilities2_runner_audio/src/audio_runners.cpp +++ /dev/null @@ -1,11 +0,0 @@ -#include -#include -#include - -namespace capabilities2_runner -{ - -} - -// register runner plugins -PLUGINLIB_EXPORT_CLASS(capabilities2_runner::VoiceListenerRunner, capabilities2_runner::RunnerBase) diff --git a/capabilities2_runner_bt/.clang-format b/capabilities2_runner_bt/.clang-format deleted file mode 100644 index d36804f..0000000 --- a/capabilities2_runner_bt/.clang-format +++ /dev/null @@ -1,64 +0,0 @@ - -BasedOnStyle: Google -AccessModifierOffset: -2 -ConstructorInitializerIndentWidth: 2 -AlignEscapedNewlinesLeft: false -AlignTrailingComments: true -AllowAllParametersOfDeclarationOnNextLine: false -AllowShortIfStatementsOnASingleLine: false -AllowShortLoopsOnASingleLine: false -AllowShortFunctionsOnASingleLine: None -AlwaysBreakTemplateDeclarations: true -AlwaysBreakBeforeMultilineStrings: false -BreakBeforeBinaryOperators: false -BreakBeforeTernaryOperators: false -BreakConstructorInitializersBeforeComma: true -BinPackParameters: true -ColumnLimit: 120 -ConstructorInitializerAllOnOneLineOrOnePerLine: true -DerivePointerBinding: false -PointerBindsToType: true -ExperimentalAutoDetectBinPacking: false -IndentCaseLabels: true -MaxEmptyLinesToKeep: 1 -NamespaceIndentation: None -ObjCSpaceBeforeProtocolList: true -PenaltyBreakBeforeFirstCallParameter: 19 -PenaltyBreakComment: 60 -PenaltyBreakString: 1 -PenaltyBreakFirstLessLess: 1000 -PenaltyExcessCharacter: 1000 -PenaltyReturnTypeOnItsOwnLine: 90 -SpacesBeforeTrailingComments: 2 -Cpp11BracedListStyle: false -Standard: Auto -IndentWidth: 2 -TabWidth: 2 -UseTab: Never -IndentFunctionDeclarationAfterType: false -SpacesInParentheses: false -SpacesInAngles: false -SpaceInEmptyParentheses: false -SpacesInCStyleCastParentheses: false -SpaceAfterControlStatementKeyword: true -SpaceBeforeAssignmentOperators: true -ContinuationIndentWidth: 4 -SortIncludes: false -SpaceAfterCStyleCast: false - -# Configure each individual brace in BraceWrapping -BreakBeforeBraces: Custom - -# Control of individual brace wrapping cases -BraceWrapping: { - AfterClass: 'true' - AfterControlStatement: 'true' - AfterEnum : 'true' - AfterFunction : 'true' - AfterNamespace : 'true' - AfterStruct : 'true' - AfterUnion : 'true' - BeforeCatch : 'true' - BeforeElse : 'true' - IndentBraces : 'false' -} diff --git a/capabilities2_runner_bt/CMakeLists.txt b/capabilities2_runner_bt/CMakeLists.txt deleted file mode 100644 index aae09e2..0000000 --- a/capabilities2_runner_bt/CMakeLists.txt +++ /dev/null @@ -1,60 +0,0 @@ -cmake_minimum_required(VERSION 3.10) -project(capabilities2_runner_bt) - -# Default to C++17 -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) -endif() - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -find_package(ament_cmake REQUIRED) -find_package(rclcpp REQUIRED) -find_package(rclcpp_action REQUIRED) -find_package(pluginlib REQUIRED) -find_package(capabilities2_msgs REQUIRED) -find_package(capabilities2_runner REQUIRED) -find_package(behaviortree_cpp REQUIRED) -find_package(PkgConfig) -pkg_check_modules(TINYXML2 tinyxml2 REQUIRED) - -include_directories(include -${TINYXML2_INCLUDE_DIRS} -) - -add_library(${PROJECT_NAME} SHARED - src/bt_runners.cpp -) - -target_link_libraries(${PROJECT_NAME} - ${TINYXML2_LIBRARIES} -) - -ament_target_dependencies(${PROJECT_NAME} - rclcpp - rclcpp_action - pluginlib - capabilities2_msgs - capabilities2_runner - behaviortree_cpp - TINYXML2 -) - -pluginlib_export_plugin_description_file(${PROJECT_NAME} plugins.xml) - -install(TARGETS ${PROJECT_NAME} - EXPORT ${PROJECT_NAME} - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION bin -) - -install(DIRECTORY include/ - DESTINATION include -) - -ament_export_include_directories(include) -ament_export_libraries(${PROJECT_NAME}) -ament_package() diff --git a/capabilities2_runner_bt/include/capabilities2_runner_bt/bt_factory_runner.hpp b/capabilities2_runner_bt/include/capabilities2_runner_bt/bt_factory_runner.hpp deleted file mode 100644 index b26ebc0..0000000 --- a/capabilities2_runner_bt/include/capabilities2_runner_bt/bt_factory_runner.hpp +++ /dev/null @@ -1,89 +0,0 @@ -#pragma once - -#include -#include -#include - -namespace capabilities2_runner -{ - -/** - * @brief base runner class for a behaviortree factory - * - * The runner implements a behaviour defined by a behaviourtree - * and executes it in a behaviortree - * - */ -class BTRunnerBase : public RunnerBase, public BT::BehaviorTreeFactory -{ -public: - BTRunnerBase() : RunnerBase(), BT::BehaviorTreeFactory() - { - } - - virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config, - std::function on_started = nullptr, - std::function on_terminated = nullptr, - std::function on_stopped = nullptr) - { - // init the runner base - init_base(node, run_config, on_started, on_terminated, on_stopped); - - // register (bt)actions from ROS plugins - try - { - this->registerFromROSPlugins(); - } - catch (const std::exception& e) // TODO: add more specific exception - { - throw runner_exception(e.what()); - } - } - - virtual void stop() - { - // reset the tree (this destroys the nodes) - tree_.reset(); - } - - /** - * @brief trigger the behaviour factory with the input data - * - * @param parameters - * @return std::optional)>> - */ - virtual std::optional)>> - trigger(std::shared_ptr parameters) - { - // if parameters are not provided then cannot proceed - if (!parameters) - throw runner_exception("cannot trigger action without parameters"); - - // create the tree (ptr) - tree_ = std::make_shared(this->createTreeFromText(parameters->GetText())); - - // return the tick function - // the caller can call this function to tick the tree - std::function)> result_callback = - [this](std::shared_ptr result) { - // tick the tree - BT::NodeStatus status = tree_->tickWhileRunning(); - - // fill the result - if (status == BT::NodeStatus::SUCCESS) - result->SetText("OK"); - else - result->SetText("FAIL"); - - return; - }; - - return result_callback; - } - -protected: - // the tree - std::shared_ptr tree_; -}; - -} // namespace capabilities2_runner diff --git a/capabilities2_runner_bt/include/capabilities2_runner_bt/bt_runner_base.hpp b/capabilities2_runner_bt/include/capabilities2_runner_bt/bt_runner_base.hpp deleted file mode 100644 index 8bf4dea..0000000 --- a/capabilities2_runner_bt/include/capabilities2_runner_bt/bt_runner_base.hpp +++ /dev/null @@ -1,109 +0,0 @@ -#pragma once - -/** - * @file bt_runner_base.hpp - * @brief This file is heavily inspired by the nav2_behavior_tree::BtActionNode implementation - * as it performs almost the same functionality - * - * The nav2_behavior_tree::BtActionNode is part of navigation2 - * which is licensed under the Apache 2.0 license. see the following - * license file for more details: - * - */ - -// Copyright (c) 2018 Intel Corporation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include -#include - -namespace capabilities2_runner -{ - -/** - * @brief base class for a behaviortree action - * - * This class is heavily inspired by the nav2_behavior_tree::BtActionNode - * - * The action implements a action runner and an action node to combine the two - * domain definitions of 'action' - * - * Through this class, an action can be run in a behaviortree which implements a ROS action - * - * @tparam ActionT - */ -template -class BTRunnerBase : public ActionRunner, public BT::ActionNodeBase -{ -public: - BTRunnerBase() : ActionRunner(), BT::ActionNodeBase() - { - } - - // init pattern for bt and runner - /** - * @brief initializer function for initializing the action runner in place of constructor due to plugin semantics - * - * TODO: this is getting pretty messy - * - * @param node - * @param run_config - * @param action_name - * @param on_started - * @param on_terminated - * @param on_stopped - * @param bt_tag_name - * @param conf - */ - virtual void init_bt(rclcpp::Node::SharedPtr node, const runner_opts& run_config, const std::string& action_name, - const std::string& bt_tag_name, const BT::NodeConfiguration& conf, - std::function on_started = nullptr, - std::function on_terminated = nullptr, - std::function on_stopped = nullptr) - { - ActionRunner::init_action(node, run_config, action_name, on_started, on_terminated, on_stopped); - - // FIXME: no init in action base - // BT::ActionNodeBase::init(bt_tag_name, conf); - } - - // bt methods that need to be overridden - BT::NodeStatus tick() override - { - // TODO: run the action client? - } - - void halt() override - { - // cancel the action client if it is running - // can be implemented with the stop virtual method - // from action runner - ActionRunner::stop(); - } - - // the action runner methods that need to be overridden - // these can be overridden when this class is inherited and the action runner - // action template is resolved - // - // see runner_base.hpp for more details - // - // virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config, - // std::function on_started = nullptr, - // std::function on_terminated = nullptr, - // std::function on_stopped = nullptr) override - // - // virtual void trigger(std::shared_ptr parameters = nullptr) override -}; - -} // namespace capabilities2_runner diff --git a/capabilities2_runner_bt/plugins.xml b/capabilities2_runner_bt/plugins.xml deleted file mode 100644 index 1289f20..0000000 --- a/capabilities2_runner_bt/plugins.xml +++ /dev/null @@ -1,6 +0,0 @@ - - - - - - diff --git a/capabilities2_runner_bt/readme.md b/capabilities2_runner_bt/readme.md deleted file mode 100644 index 16d8543..0000000 --- a/capabilities2_runner_bt/readme.md +++ /dev/null @@ -1,35 +0,0 @@ -# capabilities2_runner_bt - -Behavior tree runners for [capabilities2](../readme.md). This package provides a runner for the capabilities2 package that uses behavior trees to combine and execute composite runners in the capabilities framework. - -## Runners - -| Runner | Description | -| ------ | ----------- | -| `BTRunnerBase` | A base class for behavior tree runners. This class is used to implement the behavior tree runner for the capabilities2 package. It inherits from the [`ActionRunner`](../capabilities2_runner/readme.md) to join ROS action and behavior tree action concepts. | -| `BTFactoryRunner` | Load a behavior tree and run it as a capabilities2 runner (this just means that it runs in the capabilities2_server node). | - -### Provider Example - -The following example demonstrates how to use the `BTFactoryRunner` to load a behavior tree from a file and run it as a capabilities2 runner. - -```yaml -name: bt_ask_help -spec_version: "1.1" -spec_type: provider -implements: std_capabilities/ask_help -runner: capabilities2_runner_bt/BTFactoryRunner - -# the tree to load -definition: | - - - - - - - - - - -``` diff --git a/capabilities2_runner_bt/src/bt_runners.cpp b/capabilities2_runner_bt/src/bt_runners.cpp deleted file mode 100644 index 3a630db..0000000 --- a/capabilities2_runner_bt/src/bt_runners.cpp +++ /dev/null @@ -1,9 +0,0 @@ -#include -#include - -namespace capabilities2_runner -{ - -} // namespace capabilities2_runner - -// PLUGINLIB_EXPORT_CLASS(capabilities2_runner::BTRunnerBase, capabilities2_runner::RunnerBase) diff --git a/capabilities2_runner_nav2/.clang-format b/capabilities2_runner_nav2/.clang-format deleted file mode 100644 index d36804f..0000000 --- a/capabilities2_runner_nav2/.clang-format +++ /dev/null @@ -1,64 +0,0 @@ - -BasedOnStyle: Google -AccessModifierOffset: -2 -ConstructorInitializerIndentWidth: 2 -AlignEscapedNewlinesLeft: false -AlignTrailingComments: true -AllowAllParametersOfDeclarationOnNextLine: false -AllowShortIfStatementsOnASingleLine: false -AllowShortLoopsOnASingleLine: false -AllowShortFunctionsOnASingleLine: None -AlwaysBreakTemplateDeclarations: true -AlwaysBreakBeforeMultilineStrings: false -BreakBeforeBinaryOperators: false -BreakBeforeTernaryOperators: false -BreakConstructorInitializersBeforeComma: true -BinPackParameters: true -ColumnLimit: 120 -ConstructorInitializerAllOnOneLineOrOnePerLine: true -DerivePointerBinding: false -PointerBindsToType: true -ExperimentalAutoDetectBinPacking: false -IndentCaseLabels: true -MaxEmptyLinesToKeep: 1 -NamespaceIndentation: None -ObjCSpaceBeforeProtocolList: true -PenaltyBreakBeforeFirstCallParameter: 19 -PenaltyBreakComment: 60 -PenaltyBreakString: 1 -PenaltyBreakFirstLessLess: 1000 -PenaltyExcessCharacter: 1000 -PenaltyReturnTypeOnItsOwnLine: 90 -SpacesBeforeTrailingComments: 2 -Cpp11BracedListStyle: false -Standard: Auto -IndentWidth: 2 -TabWidth: 2 -UseTab: Never -IndentFunctionDeclarationAfterType: false -SpacesInParentheses: false -SpacesInAngles: false -SpaceInEmptyParentheses: false -SpacesInCStyleCastParentheses: false -SpaceAfterControlStatementKeyword: true -SpaceBeforeAssignmentOperators: true -ContinuationIndentWidth: 4 -SortIncludes: false -SpaceAfterCStyleCast: false - -# Configure each individual brace in BraceWrapping -BreakBeforeBraces: Custom - -# Control of individual brace wrapping cases -BraceWrapping: { - AfterClass: 'true' - AfterControlStatement: 'true' - AfterEnum : 'true' - AfterFunction : 'true' - AfterNamespace : 'true' - AfterStruct : 'true' - AfterUnion : 'true' - BeforeCatch : 'true' - BeforeElse : 'true' - IndentBraces : 'false' -} diff --git a/capabilities2_runner_nav2/CMakeLists.txt b/capabilities2_runner_nav2/CMakeLists.txt deleted file mode 100644 index 3057740..0000000 --- a/capabilities2_runner_nav2/CMakeLists.txt +++ /dev/null @@ -1,63 +0,0 @@ -cmake_minimum_required(VERSION 3.10) -project(capabilities2_runner_nav2) - -# Default to C++17 -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) -endif() - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -find_package(ament_cmake REQUIRED) -find_package(rclcpp REQUIRED) -find_package(rclcpp_action REQUIRED) -find_package(pluginlib REQUIRED) -find_package(nav2_msgs REQUIRED) -find_package(geometry_msgs REQUIRED) -find_package(capabilities2_msgs REQUIRED) -find_package(capabilities2_runner REQUIRED) -find_package(PkgConfig) -pkg_check_modules(TINYXML2 tinyxml2 REQUIRED) - -include_directories(include -${TINYXML2_INCLUDE_DIRS} -) - -add_library(${PROJECT_NAME} SHARED - src/nav2_runners.cpp -) - -target_link_libraries(${PROJECT_NAME} - ${TINYXML2_LIBRARIES} -) - -ament_target_dependencies(${PROJECT_NAME} - rclcpp - rclcpp_action - pluginlib - nav2_msgs - geometry_msgs - capabilities2_msgs - capabilities2_runner - TINYXML2 -) - -pluginlib_export_plugin_description_file(${PROJECT_NAME} plugins.xml) - -install(TARGETS ${PROJECT_NAME} - EXPORT ${PROJECT_NAME} - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION bin -) - -install(DIRECTORY include/ - DESTINATION include -) - -ament_export_include_directories(include) -ament_export_libraries(${PROJECT_NAME}) - -ament_package() diff --git a/capabilities2_runner_nav2/include/capabilities2_runner_nav2/waypoint_runner.hpp b/capabilities2_runner_nav2/include/capabilities2_runner_nav2/waypoint_runner.hpp deleted file mode 100644 index 36b5310..0000000 --- a/capabilities2_runner_nav2/include/capabilities2_runner_nav2/waypoint_runner.hpp +++ /dev/null @@ -1,123 +0,0 @@ -#pragma once - -#include - -#include -#include -#include - -#include -#include - -#include - -namespace capabilities2_runner -{ - -/** - * @brief action runner base class - * - * Class to run waypointfollower action based capability - * - */ -class WayPointRunner : public ActionRunner -{ -public: - WayPointRunner() : ActionRunner() - { - } - - /** - * @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 on_started function pointer to trigger at the start of the action client in the runner - * @param on_terminated function pointer to trigger at the termination of the action client in the runner - */ - virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config, - std::function on_started = nullptr, - std::function on_terminated = nullptr, - std::function on_stopped = nullptr) override - { - init_action(node, run_config, "follow_waypoints", on_started, on_terminated, on_stopped); - } - - /** - * @brief trigger the runner - * - @param parameters XMLElement that contains parameters in the format '' - */ - virtual std::optional)>> - trigger(std::shared_ptr parameters = nullptr) - { - tinyxml2::XMLElement* parametersElement = parameters->FirstChildElement("waypointfollower"); - - parametersElement->QueryDoubleAttribute("x", &x); - parametersElement->QueryDoubleAttribute("y", &y); - - nav2_msgs::action::FollowWaypoints::Goal goal_msg; - geometry_msgs::msg::PoseStamped pose_msg; - - global_frame_ = "map"; - robot_base_frame_ = "base_link"; - - pose_msg.header.stamp = node_->get_clock()->now(); - pose_msg.header.frame_id = global_frame_; - pose_msg.pose.position.x = x; - pose_msg.pose.position.y = y; - pose_msg.pose.position.z = 0.0; - - goal_msg.poses.push_back(pose_msg); - - auto goal_handle_future = action_client_->async_send_goal(goal_msg); - - return [this, goal_handle_future](std::shared_ptr result) { - if (rclcpp::spin_until_future_complete(node_, goal_handle_future) != rclcpp::FutureReturnCode::SUCCESS) - { - RCLCPP_ERROR(node_->get_logger(), "send goal call failed"); - return; - } - - auto result_future = action_client_->async_get_result(goal_handle_future.get()); - if (rclcpp::spin_until_future_complete(node_, result_future) != rclcpp::FutureReturnCode::SUCCESS) - { - RCLCPP_ERROR(node_->get_logger(), "get result call failed"); - return; - } - - auto wrapped_result = result_future.get(); - if (wrapped_result.code == rclcpp_action::ResultCode::SUCCEEDED) - { - RCLCPP_INFO(node_->get_logger(), "Waypoint reached"); - result->BoolAttribute("result", true); - } - else - { - RCLCPP_INFO(node_->get_logger(), "Waypoint not reached"); - } - }; - } - -protected: - // not implemented - virtual nav2_msgs::action::FollowWaypoints::Goal - generate_goal(std::shared_ptr parameters) override - { - return nav2_msgs::action::FollowWaypoints::Goal(); - } - - virtual std::shared_ptr - generate_result(const nav2_msgs::action::FollowWaypoints::Result::SharedPtr& result) override - { - return nullptr; - } - -protected: - std::string global_frame_; /**The global frame of the robot*/ - std::string robot_base_frame_; /**The frame of the robot base*/ - - double x, y; /**Coordinate frame parameters*/ -}; - -} // namespace capabilities2_runner diff --git a/capabilities2_runner_nav2/interfaces/nav2.yaml b/capabilities2_runner_nav2/interfaces/nav2.yaml deleted file mode 100644 index 1acd342..0000000 --- a/capabilities2_runner_nav2/interfaces/nav2.yaml +++ /dev/null @@ -1,15 +0,0 @@ -# standardised nav2 interface specification -%YAML 1.1 ---- -name: nav2 -spec_version: 1 -spec_type: interface -description: "Navigational capabilities using Nav2 stack" -interface: - actions: - "follow_waypoints": - type: "nav2_msgs::action::FollowWaypoints" - description: "This system allow the robot to navigate to a given two dimensional coordinate - given via '' command. '$value' represents - a value in meters. As an example ' means the - robot will move 1.2 meters forward and 0.8 meters to the right side." \ No newline at end of file diff --git a/capabilities2_runner_nav2/package.xml b/capabilities2_runner_nav2/package.xml deleted file mode 100644 index 4f32f58..0000000 --- a/capabilities2_runner_nav2/package.xml +++ /dev/null @@ -1,29 +0,0 @@ - - - capabilities2_runner_nav2 - 0.0.0 - TODO: Package description - kalana - MIT - - ament_cmake - - rclcpp - rclcpp_action - pluginlib - std_msgs - nav2_msgs - geometry_msgs - capabilities2_msgs - capabilities2_runner - - - tinyxml2 - - ament_lint_auto - ament_lint_common - - - ament_cmake - - diff --git a/capabilities2_runner_nav2/plugins.xml b/capabilities2_runner_nav2/plugins.xml deleted file mode 100644 index a0d5780..0000000 --- a/capabilities2_runner_nav2/plugins.xml +++ /dev/null @@ -1,7 +0,0 @@ - - - - A plugin that provide nav2 waypoint navigation capability - - - diff --git a/capabilities2_runner_nav2/src/nav2_runners.cpp b/capabilities2_runner_nav2/src/nav2_runners.cpp deleted file mode 100644 index a457865..0000000 --- a/capabilities2_runner_nav2/src/nav2_runners.cpp +++ /dev/null @@ -1,11 +0,0 @@ -#include -#include -#include - -namespace capabilities2_runner -{ - -} - -// register runner plugins -PLUGINLIB_EXPORT_CLASS(capabilities2_runner::WayPointRunner, capabilities2_runner::RunnerBase) diff --git a/capabilities2_server/CMakeLists.txt b/capabilities2_server/CMakeLists.txt index f219f35..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 @@ -14,17 +14,17 @@ endif() find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_action REQUIRED) +find_package(rclcpp_components REQUIRED) find_package(bondcpp REQUIRED) find_package(pluginlib REQUIRED) -find_package(rclcpp_components REQUIRED) find_package(capabilities2_msgs REQUIRED) +find_package(capabilities2_events REQUIRED) find_package(capabilities2_runner REQUIRED) -# find_package(uuid REQUIRED) -# find_package(TinyXML2 REQUIRED) +find_package(tinyxml2_vendor REQUIRED) +find_package(TinyXML2 REQUIRED) # provided by tinyxml2 upstream, or tinyxml2_vendor -find_package(PkgConfig) -pkg_check_modules(TINYXML2 tinyxml2 REQUIRED) -pkg_check_modules(UUID REQUIRED uuid) +# find_package(PkgConfig) +# pkg_check_modules(UUID REQUIRED uuid) # Find SQLite3 find_package(SQLite3) @@ -32,63 +32,102 @@ find_package(SQLite3) # find yaml-cpp find_package(yaml-cpp REQUIRED) -include_directories( - include +include_directories(include ${SQLITE3_INCLUDE_DIRS} ${YAML_CPP_INCLUDE_DIRS} - ${TINYXML2_INCLUDE_DIRS} - ${UUID_INCLUDE_DIRS} + # ${UUID_INCLUDE_DIRS} ) -add_library(${PROJECT_NAME} SHARED - src/capabilities_server.cpp +####################################################################### +# Component based implementation that compiles as a library +####################################################################### + +add_library(${PROJECT_NAME}_comp SHARED + src/capabilities_server_component.cpp ) -target_link_libraries(${PROJECT_NAME} +target_link_libraries(${PROJECT_NAME}_comp ${SQLITE3_LIBRARIES} ${YAML_CPP_LIBRARIES} - ${TINYXML2_LIBRARIES} - ${UUID_LIBRARIES} + # ${UUID_LIBRARIES} ) -ament_target_dependencies(${PROJECT_NAME} +ament_target_dependencies(${PROJECT_NAME}_comp rclcpp rclcpp_action bondcpp pluginlib rclcpp_components capabilities2_msgs + capabilities2_events capabilities2_runner + TinyXML2 SQLite3 yaml-cpp - TINYXML2 - UUID + # UUID ) -rclcpp_components_register_node(${PROJECT_NAME} +rclcpp_components_register_node(${PROJECT_NAME}_comp PLUGIN "capabilities2_server::CapabilitiesServer" - EXECUTABLE capabilities2_server_node + EXECUTABLE capabilities2_server_component ) -ament_export_targets(capabilities2_server_node) +ament_export_targets(capabilities2_server_component) -install(TARGETS ${PROJECT_NAME} - EXPORT capabilities2_server_node +install(TARGETS ${PROJECT_NAME}_comp + EXPORT capabilities2_server_component ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin ) +############################################################################ +# node implementation that compiles as a executable +############################################################################ + +add_executable(${PROJECT_NAME}_node + src/capabilities_server_node.cpp +) + +target_link_libraries(${PROJECT_NAME}_node + # ${UUID_LIBRARIES} + ${SQLITE3_LIBRARIES} + ${YAML_CPP_LIBRARIES} +) + +ament_target_dependencies(${PROJECT_NAME}_node + rclcpp + rclcpp_action + bondcpp + pluginlib + capabilities2_msgs + capabilities2_events + capabilities2_runner + TinyXML2 + SQLite3 + yaml-cpp + # UUID +) + +install(TARGETS ${PROJECT_NAME}_node + DESTINATION lib/${PROJECT_NAME} +) + # make test executable + add_executable(test_capabilities_server test/test.cpp) + target_link_libraries(test_capabilities_server - ${PROJECT_NAME} + ${PROJECT_NAME}_comp ${SQLITE3_LIBRARIES} ${YAML_CPP_LIBRARIES} - ${TINYXML2_LIBRARIES} - ${UUID_LIBRARIES} + # ${UUID_LIBRARIES} ) -add_dependencies(test_capabilities_server ${PROJECT_NAME}) +add_dependencies(test_capabilities_server ${PROJECT_NAME}_comp) + +############################################################################ +# miscellaneous +############################################################################ # install test executable install(TARGETS test_capabilities_server diff --git a/capabilities2_server/config/capabilities.yaml b/capabilities2_server/config/capabilities.yaml index be5d1e5..b0f1a87 100644 --- a/capabilities2_server/config/capabilities.yaml +++ b/capabilities2_server/config/capabilities.yaml @@ -2,11 +2,8 @@ ros__parameters: loop_rate: 5.0 # Hz db_file: ~/.ros/capabilities/capabilities.sqlite3 - rebuild: false # Set to true to rebuild the database + rebuild: true # Set to true to rebuild the database package_paths: # paths to search for capabilities + - install/ - /opt/ros/humble/share - /opt/ros/jazzy/share - - /home/$USER/capabilities_ws/src - - /home/$USER/capabilities_ws/src/capabilities2 - - /home/ubuntu/colcon_ws/src - - /home/ubuntu/colcon_ws/src/capabilities2 diff --git a/capabilities2_server/docs/register.md b/capabilities2_server/docs/register.md new file mode 100644 index 0000000..b5e9578 --- /dev/null +++ b/capabilities2_server/docs/register.md @@ -0,0 +1,22 @@ +## Registering a capability + +Capabilities can be registered by exporting them in a package. The capabilities2 server will read the capabilities from the package and make them available to the user. Basic capability use case needs two components to be registered; `capability_interface` and `capability_provider`. Optionally, a third component can be added as `semantic_capability_interface`. + +```xml + + + + interfaces/cool_cap.yaml + + + + providers/cool_cap.yaml + + + + semantic_interfaces/not_cool_cap.yaml + + +``` + +Capabilities can also be registered through a service call. This is useful for registering capabilities that are not exported in a package. The service call has been implemented as a node in the capabilities2 package. Use the node to register capabilities during runtime. This method can also be used to update capabilities. diff --git a/capabilities2_server/docs/terminal_usage.md b/capabilities2_server/docs/terminal_usage.md new file mode 100644 index 0000000..20751db --- /dev/null +++ b/capabilities2_server/docs/terminal_usage.md @@ -0,0 +1,26 @@ +## Terminal based capability usage + +Using a capability requires the user to establish a bond with the capabilities2 server. The bond is used to maintain a persistent connection with the server. + +first, inspect the available capabilities provided under this server on this robot. + +```bash +ros2 service call /capabilities/get_capability_specs capabilities2_msgs/srv/GetCapabilitySpecs +``` + +then, request a bond id to become a persistent user + +```bash +ros2 service call /capabilities/establish_bond capabilities2_msgs/srv/EstablishBond + +# this bond needs to be updated every second by publishing a heartbeat the bond topic. Replace the ... with value received from above call +ros2 topic pub /capabilities/bonds ... +``` + +then request to use a capability + +```bash +ros2 service call /capabilities/use_capability capabilities2_msgs/srv/UseCapability +``` + +This capability can be freed by calling the `free_capability` service, or just let the bond expire. The capability will be freed automatically. diff --git a/capabilities2_server/include/capabilities2_server/bond_cache.hpp b/capabilities2_server/include/capabilities2_server/bond_cache.hpp index 296ddcc..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: - BondCache(const std::string& bonds_topic = "~/bonds") : bonds_topic_(bonds_topic) + /** + * @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,10 +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); @@ -110,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) @@ -124,6 +171,8 @@ class BondCache live_bonds_[bond_id] = std::make_unique(bonds_topic_, bond_id, node, on_broken, on_formed); // start the bond + live_bonds_[bond_id]->setHeartbeatPeriod(0.10); + live_bonds_[bond_id]->setHeartbeatTimeout(10.0); live_bonds_[bond_id]->start(); } diff --git a/capabilities2_server/include/capabilities2_server/capabilities_api.hpp b/capabilities2_server/include/capabilities2_server/capabilities_api.hpp index 771a595..7413366 100644 --- a/capabilities2_server/include/capabilities2_server/capabilities_api.hpp +++ b/capabilities2_server/include/capabilities2_server/capabilities_api.hpp @@ -6,19 +6,22 @@ #include #include -#include -#include #include +#include #include + #include #include #include #include +#include +#include + #include +#include #include -#include #include #include @@ -35,7 +38,7 @@ namespace capabilities2_server class CapabilitiesAPI { public: - CapabilitiesAPI() + CapabilitiesAPI() : event_(nullptr), cap_db_(nullptr), bond_cache_(), runner_cache_(), logging_(nullptr) { } @@ -43,36 +46,18 @@ class CapabilitiesAPI * @brief connect with the database file * * @param db_file file path of the database file - * @param node_logging_interface_ptr pointer to the ROS node logging interface + * @param logging pointer to the node logging interface for logging */ - void connect(const std::string& db_file, - rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_interface_ptr) + void connect(const std::string& db_file, rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logging) { // set logger - node_logging_interface_ptr_ = node_logging_interface_ptr; + logging_ = logging; // connect db cap_db_ = std::make_unique(db_file); // log - RCLCPP_INFO(node_logging_interface_ptr_->get_logger(), "CAPAPI connected to db: %s", db_file.c_str()); - } - - /** - * @brief initialize runner events. THese are called from within runners and passed on to those execution levels - * as function pointers - * - * @param on_started event triggered at the start of the runner - * @param on_stopped event triggered at the shutdown of the runner by the capabilities server - * @param on_terminated event triggered at the failure of the runner by the action or server side. - */ - void init_events(std::function on_started, - std::function on_stopped, - std::function on_terminated) - { - runner_cache_.set_on_started(on_started); - runner_cache_.set_on_stopped(on_stopped); - runner_cache_.set_on_terminated(on_terminated); + RCLCPP_INFO(logging_->get_logger(), "Capabilities API connected to db: %s", db_file.c_str()); } /** @@ -81,9 +66,15 @@ 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` */ - void start_capability(rclcpp::Node::SharedPtr node, const std::string& capability, const std::string& provider) + bool start_capability(rclcpp::Node::SharedPtr node, const std::string& capability, const std::string& provider) { + // return value + bool value = true; + // get the running model from the db models::running_model_t running = cap_db_->get_running(provider); @@ -91,57 +82,37 @@ class CapabilitiesAPI // go through the running model and start the necessary dependencies for (const auto& run : running.dependencies) { - RCLCPP_INFO(node_logging_interface_ptr_->get_logger(), "found dependency: %s", run.interface.c_str()); + 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); // add the runner to the cache - start_capability(node, run.interface, run.provider); + value = value and start_capability(node, run.interface, run.provider); } // get the provider specification for the capability models::run_config_model_t run_config = cap_db_->get_run_config(provider); - // create a new runner - // this call implicitly starts the runner + // create a new runner, this call implicitly starts the runner // create a runner id which is the cap name to uniquely identify the runner // this means only one runner per capability name + // // TODO: consider the logic for multiple runners per capability try { runner_cache_.add_runner(node, capability, run_config); // log - RCLCPP_INFO(node_logging_interface_ptr_->get_logger(), "started capability: %s with provider: %s", - capability.c_str(), provider.c_str()); - } - catch (const capabilities2_runner::runner_exception& e) - { - RCLCPP_WARN(node_logging_interface_ptr_->get_logger(), "could not start runner: %s", e.what()); - } - } + RCLCPP_INFO(logging_->get_logger(), "started capability: %s with provider: %s", capability.c_str(), + provider.c_str()); - /** - * @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, std::shared_ptr parameters = nullptr) - { - // trigger the runner - try - { - runner_cache_.trigger_runner(capability, parameters); + return value and true; } catch (const capabilities2_runner::runner_exception& e) { - RCLCPP_WARN(node_logging_interface_ptr_->get_logger(), "could not trigger runner: %s", e.what()); + RCLCPP_WARN(logging_->get_logger(), "could not start runner: %s", e.what()); + return false; } } @@ -149,6 +120,7 @@ class CapabilitiesAPI * @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) { @@ -156,7 +128,7 @@ class CapabilitiesAPI // this can happen if dependencies fail to resolve in the first place if (!runner_cache_.running(capability)) { - RCLCPP_ERROR(node_logging_interface_ptr_->get_logger(), "could not get provider for: %s", capability.c_str()); + RCLCPP_ERROR(logging_->get_logger(), "could not get provider for: %s", capability.c_str()); return; } @@ -170,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) { - RCLCPP_INFO(node_logging_interface_ptr_->get_logger(), "freeing dependency: %s", run.interface.c_str()); + 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); @@ -190,12 +162,12 @@ class CapabilitiesAPI } catch (const capabilities2_runner::runner_exception& e) { - RCLCPP_WARN(node_logging_interface_ptr_->get_logger(), "could not stop runner: %s", e.what()); + RCLCPP_WARN(logging_->get_logger(), "could not stop runner: %s", e.what()); return; } // log - RCLCPP_INFO(node_logging_interface_ptr_->get_logger(), "stopped capability: %s", capability.c_str()); + RCLCPP_INFO(logging_->get_logger(), "stopped capability: %s", capability.c_str()); } /** @@ -214,11 +186,44 @@ class CapabilitiesAPI if (!bond_cache_.exists(capability)) { // stop the capability - RCLCPP_INFO(node_logging_interface_ptr_->get_logger(), "stopping freed capability: %s", capability.c_str()); + 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. @@ -227,18 +232,73 @@ class CapabilitiesAPI * @param capability capability name to be started * @param provider provider of the capability * @param bond_id bond_id for the capability + * + * @return `true` if capability started successfully. else returns `false` */ - void use_capability(rclcpp::Node::SharedPtr node, const std::string& capability, const std::string& provider, + bool use_capability(rclcpp::Node::SharedPtr node, const std::string& capability, const std::string& provider, const std::string& bond_id) { // add bond to cache for capability bond_cache_.add_bond(capability, bond_id); // start the capability with the provider - start_capability(node, capability, provider); + return start_capability(node, capability, provider); + } + + /** + * @brief connect RUNNING capabilities together via event type + * + * 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 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 + { + // 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; + + // 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_); + + // 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) + { + 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 @@ -252,7 +312,7 @@ class CapabilitiesAPI // exists guard if (cap_db_->exists(header.name)) { - RCLCPP_WARN(node_logging_interface_ptr_->get_logger(), "interface already exists"); + RCLCPP_WARN(logging_->get_logger(), "interface already exists: %s", header.name.c_str()); return; } @@ -264,7 +324,7 @@ class CapabilitiesAPI } catch (const std::exception& e) { - RCLCPP_ERROR(node_logging_interface_ptr_->get_logger(), "failed to convert spec to model: %s", e.what()); + RCLCPP_ERROR(logging_->get_logger(), "failed to convert spec to model: %s", e.what()); return; } @@ -273,7 +333,7 @@ class CapabilitiesAPI cap_db_->insert_interface(model); // log - RCLCPP_INFO(node_logging_interface_ptr_->get_logger(), "interface added to db: %s", model.header.name.c_str()); + RCLCPP_INFO(logging_->get_logger(), "interface added to db: %s", model.header.name.c_str()); return; } @@ -281,7 +341,7 @@ class CapabilitiesAPI { if (cap_db_->exists(header.name)) { - RCLCPP_WARN(node_logging_interface_ptr_->get_logger(), "semantic interface already exists"); + RCLCPP_WARN(logging_->get_logger(), "semantic interface already exists: %s", header.name.c_str()); return; } @@ -293,7 +353,7 @@ class CapabilitiesAPI } catch (const std::exception& e) { - RCLCPP_ERROR(node_logging_interface_ptr_->get_logger(), "failed to convert spec to model: %s", e.what()); + RCLCPP_ERROR(logging_->get_logger(), "failed to convert spec to model: %s", e.what()); return; } @@ -301,8 +361,7 @@ class CapabilitiesAPI cap_db_->insert_semantic_interface(model); // log - RCLCPP_INFO(node_logging_interface_ptr_->get_logger(), "semantic interface added to db: %s", - model.header.name.c_str()); + RCLCPP_INFO(logging_->get_logger(), "semantic interface added to db: %s", model.header.name.c_str()); return; } @@ -310,7 +369,7 @@ class CapabilitiesAPI { if (cap_db_->exists(header.name)) { - RCLCPP_WARN(node_logging_interface_ptr_->get_logger(), "provider already exists"); + RCLCPP_WARN(logging_->get_logger(), "provider already exists"); return; } @@ -322,19 +381,21 @@ class CapabilitiesAPI } catch (const std::exception& e) { - RCLCPP_ERROR(node_logging_interface_ptr_->get_logger(), "failed to convert spec to model: %s", e.what()); + RCLCPP_ERROR(logging_->get_logger(), "failed to convert spec to model: %s", e.what()); return; } + model.header.name = spec.package + "/" + model.header.name; cap_db_->insert_provider(model); // log - RCLCPP_INFO(node_logging_interface_ptr_->get_logger(), "provider added to db: %s", model.header.name.c_str()); + RCLCPP_INFO(logging_->get_logger(), "provider added to db: %s", model.header.name.c_str()); return; } // couldn't parse unknown capability type - RCLCPP_ERROR(node_logging_interface_ptr_->get_logger(), "unknown capability type: %s", spec.type.c_str()); + RCLCPP_ERROR(logging_->get_logger(), "unknown capability type: %s", spec.type.c_str()); + return; } // query api @@ -350,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; @@ -563,104 +624,17 @@ class CapabilitiesAPI return running_capabilities; } - // event api - // related to runner api - const capabilities2_msgs::msg::CapabilityEvent on_capability_started(const std::string& capability) - { - // create event msg - capabilities2_msgs::msg::CapabilityEvent event; - event.header.frame_id = "capabilities"; - event.header.stamp = rclcpp::Clock().now(); - - // started event - event.type = capabilities2_msgs::msg::CapabilityEvent::LAUNCHED; + /** */ - // set cap, prov, pid - event.capability = capability; - - // try to get details - try - { - event.provider = runner_cache_.provider(capability); - event.pid = atoi(runner_cache_.pid(capability).c_str()); - } - catch (const capabilities2_runner::runner_exception& e) - { - event.provider = "unknown"; - event.pid = -1; - } - - return event; - } - - const capabilities2_msgs::msg::CapabilityEvent on_capability_stopped(const std::string& capability) - { - // create event msg - capabilities2_msgs::msg::CapabilityEvent event; - event.header.frame_id = "capabilities"; - event.header.stamp = rclcpp::Clock().now(); - - // terminated event - event.type = capabilities2_msgs::msg::CapabilityEvent::STOPPED; - - // set cap, prov, pid - event.capability = capability; - - // try to get details - try - { - event.provider = runner_cache_.provider(capability); - event.pid = atoi(runner_cache_.pid(capability).c_str()); - } - catch (const capabilities2_runner::runner_exception& e) - { - event.provider = "unknown"; - event.pid = -1; - } - - return event; - } - - const capabilities2_msgs::msg::CapabilityEvent on_capability_terminated(const std::string& capability) - { - // create event msg - capabilities2_msgs::msg::CapabilityEvent event; - event.header.frame_id = "capabilities"; - event.header.stamp = rclcpp::Clock().now(); - - // terminated event - event.type = capabilities2_msgs::msg::CapabilityEvent::TERMINATED; - - // set cap, prov, pid - event.capability = capability; - try - { - event.provider = runner_cache_.provider(capability); - event.pid = atoi(runner_cache_.pid(capability).c_str()); - } - catch (const capabilities2_runner::runner_exception& e) - { - event.provider = "unknown"; - event.pid = -1; - } - - // log error - RCLCPP_ERROR(node_logging_interface_ptr_->get_logger(), "capability terminated: %s", capability.c_str()); - - return event; - } - - const capabilities2_msgs::msg::CapabilityEvent on_server_ready() + /** + * @brief get pid of capability + * + * @param capability capability of which the pid is requested + * @return value of pid + */ + int get_pid(const std::string& capability) { - // create event msg - capabilities2_msgs::msg::CapabilityEvent event; - event.header.frame_id = "capabilities"; - event.header.stamp = rclcpp::Clock().now(); - - // started event - event.type = capabilities2_msgs::msg::CapabilityEvent::SERVER_READY; - - return event; + return atoi(runner_cache_.pid(capability).c_str()); } // bond api @@ -668,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 @@ -682,13 +656,13 @@ class CapabilitiesAPI void on_bond_established(const std::string& bond_id) { // log bond established event - RCLCPP_INFO(node_logging_interface_ptr_->get_logger(), "bond established with id: %s", bond_id.c_str()); + 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 - RCLCPP_WARN(node_logging_interface_ptr_->get_logger(), "bond broken for id: %s", bond_id.c_str()); + 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); @@ -700,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 @@ -725,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); @@ -758,13 +717,17 @@ class CapabilitiesAPI } } -private: - // logger - rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_interface_ptr_; +protected: + // event api + std::shared_ptr event_; +private: // db std::unique_ptr cap_db_; + // for internal logging + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logging_; + // caches BondCache bond_cache_; RunnerCache runner_cache_; diff --git a/capabilities2_server/include/capabilities2_server/capabilities_db.hpp b/capabilities2_server/include/capabilities2_server/capabilities_db.hpp index db0a059..d48e7ec 100644 --- a/capabilities2_server/include/capabilities2_server/capabilities_db.hpp +++ b/capabilities2_server/include/capabilities2_server/capabilities_db.hpp @@ -13,6 +13,7 @@ #include #include #include +#include namespace capabilities2_server { @@ -475,10 +476,10 @@ class CapabilitiesDB : public DBBase interface.header.name = std::string(reinterpret_cast(sqlite3_column_text(stmt, 0))); interface.header.version = sqlite3_column_int(stmt, 1); interface.header.type = std::string(reinterpret_cast(sqlite3_column_text(stmt, 2))); - interface.header.description = std::string(reinterpret_cast(sqlite3_column_text(stmt, 3))); + interface.header.description = from_sql_safe(std::string(reinterpret_cast(sqlite3_column_text(stmt, 3)))); models::specification_model_t spec; - spec.from_yaml(YAML::Load(std::string(reinterpret_cast(sqlite3_column_text(stmt, 4))))); + spec.from_yaml(YAML::Load(from_sql_safe(std::string(reinterpret_cast(sqlite3_column_text(stmt, 4)))))); interface.interface = spec; return interface; @@ -490,11 +491,11 @@ class CapabilitiesDB : public DBBase semantic_interface.header.name = std::string(reinterpret_cast(sqlite3_column_text(stmt, 0))); semantic_interface.header.version = sqlite3_column_int(stmt, 1); semantic_interface.header.type = std::string(reinterpret_cast(sqlite3_column_text(stmt, 2))); - semantic_interface.header.description = std::string(reinterpret_cast(sqlite3_column_text(stmt, 3))); + semantic_interface.header.description = from_sql_safe(std::string(reinterpret_cast(sqlite3_column_text(stmt, 3)))); semantic_interface.redefines = std::string(reinterpret_cast(sqlite3_column_text(stmt, 4))); semantic_interface.global_namespace = std::string(reinterpret_cast(sqlite3_column_text(stmt, 5))); semantic_interface.remappings.from_yaml( - YAML::Load(std::string(reinterpret_cast(sqlite3_column_text(stmt, 6))))); + YAML::Load(from_sql_safe(std::string(reinterpret_cast(sqlite3_column_text(stmt, 6)))))); return semantic_interface; } @@ -505,11 +506,11 @@ class CapabilitiesDB : public DBBase provider.header.name = std::string(reinterpret_cast(sqlite3_column_text(stmt, 0))); provider.header.version = sqlite3_column_int(stmt, 1); provider.header.type = std::string(reinterpret_cast(sqlite3_column_text(stmt, 2))); - provider.header.description = std::string(reinterpret_cast(sqlite3_column_text(stmt, 3))); + provider.header.description = from_sql_safe(std::string(reinterpret_cast(sqlite3_column_text(stmt, 3)))); provider.implements = std::string(reinterpret_cast(sqlite3_column_text(stmt, 4))); - provider.depends_on = YAML::Load(std::string(reinterpret_cast(sqlite3_column_text(stmt, 5)))) + provider.depends_on = YAML::Load(from_sql_safe(std::string(reinterpret_cast(sqlite3_column_text(stmt, 5))))) .as>(); - provider.remappings.from_yaml(YAML::Load(std::string(reinterpret_cast(sqlite3_column_text(stmt, 6))))); + provider.remappings.from_yaml(YAML::Load(from_sql_safe(std::string(reinterpret_cast(sqlite3_column_text(stmt, 6)))))); provider.runner = std::string(reinterpret_cast(sqlite3_column_text(stmt, 7))); return provider; diff --git a/capabilities2_server/include/capabilities2_server/capabilities_server.hpp b/capabilities2_server/include/capabilities2_server/capabilities_server.hpp index 4012142..2fec6b5 100644 --- a/capabilities2_server/include/capabilities2_server/capabilities_server.hpp +++ b/capabilities2_server/include/capabilities2_server/capabilities_server.hpp @@ -7,25 +7,26 @@ #include #include #include - #include - #include #include -#include #include -#include +#include +#include + #include -#include +#include #include #include #include #include +#include #include #include +#include #include #include #include @@ -33,7 +34,6 @@ #include #include #include -#include namespace capabilities2_server { @@ -59,12 +59,32 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI { public: CapabilitiesServer(const rclcpp::NodeOptions& options = rclcpp::NodeOptions()) - : Node("capabilities", options), CapabilitiesAPI() + : Node("capabilities2", options), CapabilitiesAPI() + { + try + { + // Only call setup if this object is already owned by a shared_ptr + if (shared_from_this()) + { + initialize(); + } + } + catch (const std::bad_weak_ptr&) + { + // Not yet safe — probably standalone without make_shared + } + } + + /** + * @brief Initializes the capabilities server node. + * + */ + void initialize() { // 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"); @@ -109,8 +129,10 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI } // pubs - // events - event_pub_ = create_publisher("~/events", 10); + event_pub_ = create_publisher("~/events", 10); + + // event publisher uses event subsystem + event_ = std::make_shared(event_pub_); // subs @@ -135,11 +157,21 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI "~/free_capability", std::bind(&CapabilitiesServer::free_capability_cb, this, std::placeholders::_1, std::placeholders::_2)); + // trigger capability + trigger_capability_srv_ = create_service( + "~/trigger_capability", + std::bind(&CapabilitiesServer::trigger_capability_cb, this, std::placeholders::_1, std::placeholders::_2)); + // use capability use_capability_srv_ = create_service( "~/use_capability", std::bind(&CapabilitiesServer::use_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( "~/register_capability", @@ -174,19 +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)); - // create publishing event callbacks by binding the event publisher and event message callbacks - // handled by the API class and passed around to runners - // on started, stopped, and terminated lambdas binding event_pub_ - // init events system callbacks with lambdas - init_events([this](const std::string& cap) { event_pub_->publish(on_capability_started(cap)); }, - [this](const std::string& cap) { event_pub_->publish(on_capability_stopped(cap)); }, - [this](const std::string& cap) { event_pub_->publish(on_capability_terminated(cap)); }); - // log ready RCLCPP_INFO(get_logger(), "capabilities server started"); - // publish ready event - event_pub_->publish(on_server_ready()); + // fire/publish ready event + event_->on_server_ready("capabilities server start up complete"); } // service callbacks @@ -204,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); @@ -216,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); @@ -224,7 +256,42 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI res->successful = true; } + // trigger capability + // requires a bond to be established + void trigger_capability_cb(const std::shared_ptr req, + std::shared_ptr res) + { + // 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->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) { @@ -248,6 +315,7 @@ 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) { @@ -276,6 +344,39 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI // response is empty } + // 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) + { + // need to have a bond established + if (req->bond_id.empty()) + { + RCLCPP_ERROR(get_logger(), "connect_capability: bond_id is empty"); + return; + } + + // need to have a instance id + if (req->connection.source.instance_id.empty()) + { + RCLCPP_ERROR(get_logger(), "connect_capability: instance_id is empty"); + return; + } + + // 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; + } + + // api connect capability + connect_capability(req->bond_id, req->connection); + + // response is empty + } + // register capability void register_capability_cb(const std::shared_ptr req, std::shared_ptr res) @@ -311,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 @@ -374,43 +475,55 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI } private: - /** - * @brief Load capabilities from a package path - * - * @param package_path - */ void load_capabilities(const std::string& 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)) { RCLCPP_ERROR(get_logger(), "package path does not exist: %s", package_path.c_str()); return; } + // find packages in path - std::vector packages; + std::vector packages_root, packages_install; + for (const auto& entry : std::filesystem::directory_iterator(package_path)) { if (entry.is_directory()) { - packages.push_back(entry.path().filename()); + std::string name(entry.path().filename()); + + if (std::filesystem::exists(package_path + "/" + name + "/package.xml")) + { + packages_root.push_back(name); + } + else if (std::filesystem::exists(package_path + "/" + name + "/share/" + name + "/package.xml")) + { + packages_install.push_back(name); + } } } - // load capabilities from packages - for (const auto& package : packages) + + // load capabilities from packages in /opt/ros/*/share + for (const auto& package : packages_root) { - RCLCPP_DEBUG(get_logger(), "loading capabilities from package: %s", package.c_str()); + RCLCPP_DEBUG(get_logger(), "Loading capabilities from package: %s", package.c_str()); + // package.xml exports std::string package_xml = package_path + "/" + package + "/package.xml"; + // check if package.xml exists if (!std::filesystem::exists(package_xml)) { - RCLCPP_DEBUG(get_logger(), "package.xml does not exist: %s", package_xml.c_str()); + RCLCPP_ERROR(get_logger(), "package.xml does not exist: %s", package_xml.c_str()); continue; } + // parse package.xml tinyxml2::XMLDocument doc; + try { parse_package_xml(package_xml, doc); @@ -420,13 +533,16 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI RCLCPP_ERROR(get_logger(), "failed to parse package.xml file: %s", e.what()); continue; } + // get exports tinyxml2::XMLElement* exports = doc.FirstChildElement("package")->FirstChildElement("export"); + if (exports == nullptr) { - RCLCPP_DEBUG(get_logger(), "No exports found in package.xml file: %s", package_xml.c_str()); + RCLCPP_ERROR(get_logger(), "No exports found in package.xml file: %s", package_xml.c_str()); continue; } + // get capability specs of each type using a lambda auto get_capability_specs = [&](const std::string& spec_type) { // get capability spec @@ -452,8 +568,9 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI { // read spec file load_spec_content(package_path + "/" + package + "/" + spec_path, capability_spec); + // add capability to db - RCLCPP_INFO(get_logger(), "adding capability: %s", (package + "/" + spec_path).c_str()); + 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) @@ -462,6 +579,90 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI } } }; + + // get interface specs + get_capability_specs(capabilities2_msgs::msg::CapabilitySpec::CAPABILITY_INTERFACE); + // get semantic interface specs + get_capability_specs(capabilities2_msgs::msg::CapabilitySpec::SEMANTIC_CAPABILITY_INTERFACE); + // get provider specs + get_capability_specs(capabilities2_msgs::msg::CapabilitySpec::CAPABILITY_PROVIDER); + } + + // load capabilities from packages in workspace install folder + for (const auto& package : packages_install) + { + 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"; + + // check if package.xml exists + if (!std::filesystem::exists(package_xml)) + { + RCLCPP_ERROR(get_logger(), "package.xml does not exist: %s", package_xml.c_str()); + continue; + } + + // parse package.xml + tinyxml2::XMLDocument doc; + + try + { + parse_package_xml(package_xml, doc); + } + catch (const std::runtime_error& e) + { + RCLCPP_ERROR(get_logger(), "failed to parse package.xml file: %s", e.what()); + continue; + } + + // get exports + tinyxml2::XMLElement* exports = doc.FirstChildElement("package")->FirstChildElement("export"); + + if (exports == nullptr) + { + RCLCPP_ERROR(get_logger(), "No exports found in package.xml file: %s", package_xml.c_str()); + continue; + } + + // get capability specs of each type using a lambda + auto get_capability_specs = [&](const std::string& spec_type) { + // get capability spec + for (tinyxml2::XMLElement* spec = exports->FirstChildElement(spec_type.c_str()); spec != nullptr; + spec = spec->NextSiblingElement(spec_type.c_str())) + { + // read spec relative path for spec file from element contents + std::string spec_path = spec->GetText(); + // clear white spaces + spec_path.erase(std::remove_if(spec_path.begin(), spec_path.end(), isspace), spec_path.end()); + // remove leading slash + if (spec_path[0] == '/') + { + spec_path = spec_path.substr(1); + } + // create a spec message + capabilities2_msgs::msg::CapabilitySpec capability_spec; + // add package details + capability_spec.package = package; + capability_spec.type = spec_type; + // try load spec file + try + { + // read spec file + load_spec_content(package_path + "/" + package + "/share/" + package + "/" + spec_path, capability_spec); + + // add capability to db + 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) + { + RCLCPP_ERROR(get_logger(), "failed to load spec file: %s", e.what()); + } + } + }; + // get interface specs get_capability_specs(capabilities2_msgs::msg::CapabilitySpec::CAPABILITY_INTERFACE); // get semantic interface specs @@ -527,12 +728,9 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI // loop hz double loop_hz_; - /** capabilities_fabric launch thread */ - std::shared_ptr fabric_launch_thread; - // publishers // event publisher - rclcpp::Publisher::SharedPtr event_pub_; + rclcpp::Publisher::SharedPtr event_pub_; // services // establish bond @@ -543,6 +741,10 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI rclcpp::Service::SharedPtr stop_capability_srv_; // free capability rclcpp::Service::SharedPtr free_capability_srv_; + // connect capability + rclcpp::Service::SharedPtr connect_capability_srv_; + // trigger capability + rclcpp::Service::SharedPtr trigger_capability_srv_; // use capability rclcpp::Service::SharedPtr use_capability_srv_; // register capability @@ -561,9 +763,6 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI rclcpp::Service::SharedPtr get_remappings_srv_; // get running capabilities rclcpp::Service::SharedPtr get_running_capabilities_srv_; - - /** action server that exposes cabapilities fabric*/ - std::shared_ptr> capabilities_fabric_server; }; } // namespace capabilities2_server diff --git a/capabilities2_server/include/capabilities2_server/models/header.hpp b/capabilities2_server/include/capabilities2_server/models/header.hpp index 6edd01d..7dc9738 100644 --- a/capabilities2_server/include/capabilities2_server/models/header.hpp +++ b/capabilities2_server/include/capabilities2_server/models/header.hpp @@ -2,6 +2,7 @@ #include #include +#include namespace capabilities2_server { @@ -70,7 +71,7 @@ struct header_model_t : identifiable_base_t, soft_deleteable_base_t, modifiable_ const std::string to_sql_values() const { - return "'" + name + "', '" + version + "', '" + type + "', '" + description + "'"; + return "'" + name + "', '" + version + "', '" + type + "', '" + to_sql_safe(description) + "'"; } }; diff --git a/capabilities2_server/include/capabilities2_server/models/interface.hpp b/capabilities2_server/include/capabilities2_server/models/interface.hpp index ba5b847..f6a47b0 100644 --- a/capabilities2_server/include/capabilities2_server/models/interface.hpp +++ b/capabilities2_server/include/capabilities2_server/models/interface.hpp @@ -5,6 +5,7 @@ #include #include #include +#include namespace capabilities2_server { @@ -164,7 +165,7 @@ struct interface_model_t : public predicateable_base_t const std::string to_sql_values() const { - return header.to_sql_values() + ", '" + YAML::Dump(interface.to_yaml()) + "'"; + return header.to_sql_values() + ", '" + to_sql_safe(YAML::Dump(interface.to_yaml())) + "'"; } }; diff --git a/capabilities2_server/include/capabilities2_server/models/provider.hpp b/capabilities2_server/include/capabilities2_server/models/provider.hpp index 1c20da6..eb64573 100644 --- a/capabilities2_server/include/capabilities2_server/models/provider.hpp +++ b/capabilities2_server/include/capabilities2_server/models/provider.hpp @@ -4,8 +4,10 @@ #include #include #include +#include #include #include +#include namespace capabilities2_server { @@ -20,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, public 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; @@ -42,6 +44,7 @@ struct provider_model_t : public remappable_base_t, predicateable_base_t, public { remappings.from_yaml(node["remappings"]); } + // definition defineable_base_t::from_yaml(node); } @@ -75,8 +78,9 @@ struct provider_model_t : public remappable_base_t, predicateable_base_t, public { YAML::Node deps; deps["depends_on"] = depends_on; - return header.to_sql_values() + ", '" + implements + "', '" + YAML::Dump(deps["depends_on"]) + "', '" + - YAML::Dump(remappings.to_yaml()) + "', '" + runner + "', '" + definition_str + "'"; + + return header.to_sql_values() + ", '" + implements + "', '" + to_sql_safe(YAML::Dump(deps["depends_on"])) + "', '" + + to_sql_safe(YAML::Dump(remappings.to_yaml())) + "', '" + runner + "', '" + definition_str + "'"; } }; diff --git a/capabilities2_server/include/capabilities2_server/models/readme.md b/capabilities2_server/include/capabilities2_server/models/readme.md new file mode 100644 index 0000000..e625a63 --- /dev/null +++ b/capabilities2_server/include/capabilities2_server/models/readme.md @@ -0,0 +1,53 @@ +# Capabilities2 Models + +This directory contains the data models used by the Capabilities2 server. + +## Models + +| Model | Description | +|-------|-------------| +| `provider` | Represents a provider of an interface | +| `interface` | Represents a capability interface | +| `semantic_interface` | Represents a semantic capability interface | + +### V2 new models (TODO) + +| Model | Description | +|-------|-------------| +| `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: + +| Trait | Description | +|-------|-------------| +| `identifiable` | Allows the model to have a unique identifier | +| `modifiable` | Adds created and modified timestamps | +| `soft_deleteable` | Allows the model to be soft deleted | +| `header` | Adds a header to the model including name, description, and version etc. The header uses id, created, and deleted timestamps | +| `remappable` | Allows the model to have remappings | + +### V2 new traits (TODO) + +| Trait | Description | +|-------|-------------| +| `predicateable` | Allows the model to use subject-predicate relationships | +| `assurable` | Adds tracking metrics for capability performance | + +## Relationships + +The models can have relationships with each other. The available relationships are: + +| Relationship | Description | +|--------------|-------------| +| `implements` | A provider implements an interface | +| `depends_on` | A provider depends on another provider | +| `redefines` | A semantic interface redefines an interface | 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/models/semantic_interface.hpp b/capabilities2_server/include/capabilities2_server/models/semantic_interface.hpp index 482ad13..0a727bd 100644 --- a/capabilities2_server/include/capabilities2_server/models/semantic_interface.hpp +++ b/capabilities2_server/include/capabilities2_server/models/semantic_interface.hpp @@ -4,6 +4,7 @@ #include #include #include +#include namespace capabilities2_server { @@ -58,7 +59,7 @@ struct semantic_interface_model_t : public remappable_base_t, predicateable_base std::string to_sql_values() const { return header.to_sql_values() + ", '" + redefines + "', '" + global_namespace + "', '" + - YAML::Dump(remappings.to_yaml()) + "'"; + to_sql_safe(YAML::Dump(remappings.to_yaml())) + "'"; } }; diff --git a/capabilities2_server/include/capabilities2_server/runner_cache.hpp b/capabilities2_server/include/capabilities2_server/runner_cache.hpp index d50b412..2689b4e 100644 --- a/capabilities2_server/include/capabilities2_server/runner_cache.hpp +++ b/capabilities2_server/include/capabilities2_server/runner_cache.hpp @@ -4,11 +4,15 @@ #include #include #include -#include +#include +// #include #include #include + #include #include +#include +#include namespace capabilities2_server { @@ -21,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 @@ -29,9 +33,6 @@ class RunnerCache public: RunnerCache() : runner_loader_("capabilities2_runner", "capabilities2_runner::RunnerBase") { - on_started = nullptr; - on_stopped = nullptr; - on_terminated = nullptr; } /** @@ -44,12 +45,10 @@ class RunnerCache void add_runner(rclcpp::Node::SharedPtr node, const std::string& capability, const models::run_config_model_t& run_config) { - // if the runner exists then throw an error + // if the runner exists then throw an error preserving uniqueness if (running(capability)) { - // already running throw capabilities2_runner::runner_exception("capability is running already: " + capability); - // return; } // check if run config is valid @@ -58,9 +57,8 @@ class RunnerCache throw capabilities2_runner::runner_exception("run config is not valid: " + YAML::Dump(run_config.to_yaml())); } - // create the runner - // add the runner to map - // if the spec runner contains a path to a launch file then use the launch file runner + // create the runner, add the runner to map, and if the spec runner contains a path to a launch file then use the + // launch file runner if (run_config.runner.find(".launch") != std::string::npos || run_config.runner.find("/") != std::string::npos || run_config.runner.find(".py") != std::string::npos) { @@ -68,12 +66,11 @@ class RunnerCache } else { - // use different runner types based on cap and provider specs runner_cache_[capability] = runner_loader_.createSharedInstance(run_config.runner); } // start the runner - runner_cache_[capability]->start(node, run_config.to_runner_opts(), on_started, on_terminated, on_stopped); + runner_cache_[capability]->start(node, run_config.to_runner_opts(), ""); } /** @@ -82,14 +79,19 @@ 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, std::shared_ptr parameters = nullptr) + 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 { @@ -97,25 +99,64 @@ class RunnerCache } } + /** + * @brief Add state change connection between runners via an event + * + * @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 add_connection(const std::string& capability, const std::string& connection_id, + const capabilities2_msgs::msg::CapabilityConnection& connection, + std::shared_ptr event_emitter = nullptr) + { + // 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) { - // find the runner in the cache + // find the runner in the cache and if not found then throw an error if (!running(capability)) { - // not found so nothing to do throw capabilities2_runner::runner_exception("capability runner not found: " + capability); - // return; } // safely stop the runner try { - runner_cache_[capability]->stop(); + runner_cache_[capability]->stop(""); } catch (const capabilities2_runner::runner_exception& e) { @@ -124,7 +165,7 @@ class RunnerCache } // reset the runner pointer - runner_cache_[capability].reset(); + // runner_cache_[capability].reset(); // remove the runner from map runner_cache_.erase(capability); @@ -199,36 +240,6 @@ class RunnerCache return runner_cache_.find(capability) != runner_cache_.end(); } - /** - * @brief Callback function for 'on_started' event - * - * @param cb callback function pointer - */ - void set_on_started(std::function cb) - { - on_started = cb; - } - - /** - * @brief Callback function for 'on_stopped' event - * - * @param cb callback function pointer - */ - void set_on_stopped(std::function cb) - { - on_stopped = cb; - } - - /** - * @brief Callback function for 'on_terminated' event - * - * @param cb callback function pointer - */ - void set_on_terminated(std::function cb) - { - on_terminated = cb; - } - private: // map capability to running model // capability / provider specs -> runner @@ -236,11 +247,6 @@ class RunnerCache // runner plugin loader pluginlib::ClassLoader runner_loader_; - - // event callbacks - std::function on_started; - std::function on_stopped; - std::function on_terminated; }; } // namespace capabilities2_server diff --git a/capabilities2_server/include/capabilities2_server/utils/sql_safe.hpp b/capabilities2_server/include/capabilities2_server/utils/sql_safe.hpp new file mode 100644 index 0000000..0f7c2c0 --- /dev/null +++ b/capabilities2_server/include/capabilities2_server/utils/sql_safe.hpp @@ -0,0 +1,47 @@ +#pragma once +#include +#include + +namespace capabilities2_server +{ + +/** + * @brief convert a generic std::string to a sql safe std::string + * + * @param input generic std::string + * @return sql safe std::string + * + */ +std::string to_sql_safe(const std::string& input) +{ + std::string result = input; + + // Escape backslashes (if necessary for your SQL dialect) + result = std::regex_replace(result, std::regex(R"(\\)"), R"(\\\\)"); + + // Escape single quotes by replacing them with two single quotes + result = std::regex_replace(result, std::regex(R"(')"), R"('')"); + + return result; +} + +/** + * @brief convert a sql safe std::string to a generic std::string + * + * @param input sql safe std::string + * @return generic std::string + * + */ +std::string from_sql_safe(const std::string& input) +{ + std::string result = input; + + // Escape backslashes (if necessary for your SQL dialect) + result = std::regex_replace(result, std::regex(R"(\\\\)"), R"(\\)"); + + // Escape single quotes by replacing them with two single quotes + result = std::regex_replace(result, std::regex(R"('')"), R"(')"); + + return result; +} +} // namespace capabilities2_server diff --git a/capabilities2_server/launch/capabilities2_server.composed.launch.py b/capabilities2_server/launch/capabilities2_server.composed.launch.py new file mode 100644 index 0000000..894a05d --- /dev/null +++ b/capabilities2_server/launch/capabilities2_server.composed.launch.py @@ -0,0 +1,47 @@ +''' +capabilities2_server launch file +''' + +import os +from launch import LaunchDescription +from launch_ros.actions import Node +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + """Generate launch description for capabilities2 server + + Returns: + LaunchDescription: The launch description for capabilities2 server + """ + # load config file + server_config = os.path.join( + get_package_share_directory('capabilities2_server'), + 'config', + 'capabilities.yaml' + ) + + # create bridge composition + capabilities = ComposableNodeContainer( + name='capabilities2_container', + namespace='', + package='rclcpp_components', + executable='component_container_mt', + # prefix=['xterm -e gdb -ex run --args'], # Add GDB debugging prefix + arguments=['--ros-args', '--log-level', 'info'], + composable_node_descriptions=[ + ComposableNode( + package='capabilities2_server', + plugin='capabilities2_server::CapabilitiesServer', + name='capabilities', + parameters=[server_config] + ) + ] + ) + + # return + return LaunchDescription([ + capabilities + ]) diff --git a/capabilities2_server/launch/capabilities2_server.launch.py b/capabilities2_server/launch/capabilities2_server.launch.py index 759947b..efcce28 100644 --- a/capabilities2_server/launch/capabilities2_server.launch.py +++ b/capabilities2_server/launch/capabilities2_server.launch.py @@ -5,8 +5,8 @@ import os from launch import LaunchDescription from launch_ros.actions import Node -from launch_ros.actions import ComposableNodeContainer -from launch_ros.descriptions import ComposableNode +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource from ament_index_python.packages import get_package_share_directory @@ -17,37 +17,23 @@ def generate_launch_description(): LaunchDescription: The launch description for capabilities2 server """ # load config file - config = os.path.join( + server_config = os.path.join( get_package_share_directory('capabilities2_server'), 'config', 'capabilities.yaml' ) - # create bridge composition - capabilities = ComposableNodeContainer( - name='capabilities2_container', - namespace='', - package='rclcpp_components', - executable='component_container', - composable_node_descriptions=[ - ComposableNode( - package='capabilities2_server', - plugin='capabilities2_server::CapabilitiesServer', - name='capabilities', - parameters=[config] - ) - ] - ) - - # create launch proxy node - launch_proxy = Node( - package='capabilities2_launch_proxy', - executable='capabilities_launch_proxy', - name='capabilities_launch_proxy' + # create cap node + capabilities2 = Node( + package='capabilities2_server', + executable='capabilities2_server_node', + name='capabilities', + parameters=[server_config], + output='screen', + arguments=['--ros-args', '--log-level', 'info'] ) # return return LaunchDescription([ - capabilities, - launch_proxy + capabilities2 ]) diff --git a/capabilities2_server/package.xml b/capabilities2_server/package.xml index ffa1e48..687fdad 100644 --- a/capabilities2_server/package.xml +++ b/capabilities2_server/package.xml @@ -1,15 +1,17 @@ capabilities2_server - 0.0.1 + 0.2.0 Package that implements the capabilities2 service, a successor to capabilities Michael Pritchard mik-p + Kalana Ratnayake Michael Pritchard + Kalana Ratnayake MIT @@ -21,20 +23,19 @@ pluginlib rclcpp_components capabilities2_msgs + capabilities2_events capabilities2_runner + tinyxml2_vendor yaml-cpp - tinyxml2 libsqlite3-dev - uuid + rclpy - launch_ros - capabilities2_launch_proxy + ament_cmake - diff --git a/capabilities2_server/readme.md b/capabilities2_server/readme.md index 62cb0db..e51f9ff 100644 --- a/capabilities2_server/readme.md +++ b/capabilities2_server/readme.md @@ -6,6 +6,19 @@ Capabilities2 server allows interaction with the capabilities2 API. It is a reim 2. Capability registration through service calls. 3. Abstracted providers through a *runners* API see [capabilities2_runner](../capabilities2_runner/readme.md) +## Specialising the capabilities2 server + +The capabilities2 server is implemented as a [ROS2 Component](https://docs.ros.org/en/jazzy/Concepts/Intermediate/About-Composition.html). The server can be specialised by composing it with another Component that adds domain logic. + +## Launch capabilities2 server + +```bash +source install/setup.bash +ros2 launch capabilities2_server server.launch.py +``` + +Refer `capabilities2_server/config/capabilities.yaml` for launch arguments. + ## System dependencies The capabilities2 server depends on the following `bondcpp` ROS2 package. See the package.xml for the full list of dependencies. Notably, the capabilities2 server depends on the following system dependencies: @@ -13,11 +26,10 @@ The capabilities2 server depends on the following `bondcpp` ROS2 package. See th - `sqlite3` - `yaml-cpp` - `tinyxml2` -- `uuid` ## API -The capabilities2 server provides the following ROS API: +The capabilities2 server provides the following ROS2 API: ### Services @@ -33,100 +45,24 @@ 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 - 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) | - -## How to use - -### Register a capability - -Capabilities can be registered by exporting them in a package. The capabilities2 server will read the capabilities from the package and make them available to the user. - -```xml - - - - interfaces/cool_cap.yaml - - -``` - -Following is the content of the cool_cap.yaml - -```yaml -# cool_cap.yaml -name: cool_cap -spec_version: 1 -spec_type: interface -description: "This is a cool capability" -interface: - topics: - "/cool_topic": - type: "std_msgs/msg/String" - description: "This is a cool topic" -``` - -Capabilities can also be registered through a service call. This is useful for registering capabilities that are not exported in a package. The service call has been implemented as a node in the capabilities2 package. Use the node to register capabilities during runtime. This method can also be used to update capabilities. - -### Launch capabilities2 server - -```bash -ros2 launch capabilities2_server capabilities2_server.launch.py -``` - -Can add launch args for path to packages for exported capabilties via terminal as well. refer `./config/capabilities.yaml` for example usage. - +| :--- | :--- | :--- | +| `~/events` | `CapabilityEventStamped.msg` | Publish capability events | +| `~/bonds` | `bond/Status.msg` | Maintain bonds with capability users - [Bond API](https://wiki.ros.org/bond) | -### Terminal based capability usage +## Additional Information -Using a capability requires the user to establish a bond with the capabilities2 server. The bond is used to maintain a persistent connection with the server. - -first, inspect the available capabilities provided under this server on this robot. - -```bash -ros2 service call /capabilities/get_capability_specs capabilities2_msgs/srv/GetCapabilitySpecs -``` - -then, request a bond id to become a persistent user - -```bash -ros2 service call /capabilities/establish_bond capabilities2_msgs/srv/EstablishBond - -# this bond needs to be updated every second by publishing a heartbeat the bond topic. Replace the ... with value received from above call -ros2 topic pub /capabilities/bonds ... -``` - -then request to use a capability - -```bash -ros2 service call /capabilities/use_capability capabilities2_msgs/srv/UseCapability -``` - -This capability can be freed by calling the `free_capability` service, or just let the bond expire. The capability will be freed automatically. - - -## Developing - -A `devcontainer` is provided for developing the capabilities2 meta-package. Dependencies need to be installed in the container. Install the dependencies with rosdep: - -```bash -# in the devcontainer -rosdep install --from-paths src --ignore-src -r -y -``` - -### Specialising the capabilities2 server - -The capabilities2 server is implemented as a [ROS2 Component](https://docs.ros.org/en/jazzy/Concepts/Intermediate/About-Composition.html). The server can be specialised by composing it with another Component that adds domain logic. +- [Registering a capability](../capabilities2_server/docs/register.md) +- [Terminal based capability usage](../capabilities2_server/docs/terminal_usage.md) diff --git a/capabilities2_server/src/capabilities_server.cpp b/capabilities2_server/src/capabilities_server_component.cpp similarity index 94% rename from capabilities2_server/src/capabilities_server.cpp rename to capabilities2_server/src/capabilities_server_component.cpp index 8c8eb6f..efbad0e 100644 --- a/capabilities2_server/src/capabilities_server.cpp +++ b/capabilities2_server/src/capabilities_server_component.cpp @@ -1,4 +1,4 @@ #include #include -RCLCPP_COMPONENTS_REGISTER_NODE(capabilities2_server::CapabilitiesServer) +RCLCPP_COMPONENTS_REGISTER_NODE(capabilities2_server::CapabilitiesServer) \ No newline at end of file diff --git a/capabilities2_server/src/capabilities_server_node.cpp b/capabilities2_server/src/capabilities_server_node.cpp new file mode 100644 index 0000000..2817989 --- /dev/null +++ b/capabilities2_server/src/capabilities_server_node.cpp @@ -0,0 +1,27 @@ +#include + +int main(int argc, char** argv) +{ + // Initialize ROS 2 + rclcpp::init(argc, argv); + + // Create the node object and executor object + auto node = std::make_shared(); + + // Initialize the node + node->initialize(); + + // Create a MultiThreadedExecutor + auto exec = std::make_shared(); + + // Add the node to the executor + exec->add_node(node); + + // Spin the executor + exec->spin(); + + // Shutdown ROS 2 + rclcpp::shutdown(); + + return 0; +} \ No newline at end of file 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/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 diff --git a/docs/design.md b/docs/design.md new file mode 100644 index 0000000..4366e1b --- /dev/null +++ b/docs/design.md @@ -0,0 +1,15 @@ +## Design + +The new capabilities package is designed to be more efficient and extensible. The functions are implemented as plugins, which can be loaded at runtime. The execution of providers is abstracted using an API called runners. The runners can manage more arbitrary provider operation which can include performing actions or services, or even running other capabilities. The capability models are stored in a database, This will allow various feature improvements such as hot reloading, state persistence, and model extension. Another possible feature is to create more complex ontological relationships between capabilities such as prerequisites, conflicts, or RDF triples (for example, `grasp` results in `holding`, or `pick` is a type of `manipulation`, or `grasp` is incompatible with `push`). + +### Bonds + +The bonds feature as implemented in `capabilities` and reimplemented in `capabilities2` allows applications to overlap functions by requesting use of the same resources. When all references to a resource are released, the resource is stopped. This has the added benefit of creating an idle state for the robot in which the robot computing resources are not being used. As a result of this, the robot could be more energy-efficient, and the robot could be more responsive to new tasks. + +### Runners + +The runners feature which is new in `capabilities2` allows capabilities to be executed in a more diverse manner. This could include running actions, services, or even other capabilities. This allows capabilities to be used like actions and not just started and stopped by the capabilities service. The Runner API is designed to be extensible, so that specific runners can be created on a per provider basis. Another feature that can be implemented is cross-runner communication, which could allow capabilities to be combined at runtime to perform more complex tasks. This could overcome limitations in robot programming in which there is a common need for extensive pre-definition of tasks. + +#### launch runner + +To enable the launch functionality from `capabilities` in `capabilities2`, a `launch runner` has been implemented. Due to the design of the launch system in ROS2, it was necessary to create a `launch_proxy` node which uses the `python` launch API to start and stop launch files. The runner allows uses an action to start and stop launch files, and keep track of running launch files. diff --git a/docs/foxglove_studio.md b/docs/foxglove_studio.md new file mode 100644 index 0000000..341ea13 --- /dev/null +++ b/docs/foxglove_studio.md @@ -0,0 +1,7 @@ +# Dependency Installation with Foxglove Studio + +We utilize foxglove studio to visalize information about the Capabilities2 system. + +Event system has been defined in capabilities2_events package and foxglove-bridge has been set as a dependency of capabilities2_events for ease of installation. We have selected foxglove-bridge over rosbridge due to performance improvements foxglove-bridge has over rosbridge due to former being written in C++ and latter being in Python. + +To visualize data, download foxglove-studio from [website](https://foxglove.dev/download) and create a free account when signing in. \ No newline at end of file diff --git a/docs/run_test_scripts.md b/docs/run_test_scripts.md index 791200c..77044e3 100644 --- a/docs/run_test_scripts.md +++ b/docs/run_test_scripts.md @@ -1,14 +1,23 @@ # Run test scripts -Capabilities features can be tested using the provided test scripts. The test scripts primarily test the capabilities2 server using service clients. Launch the capabilities2 server before running the test scripts [information here](../capabilities2_server/readme.md). The test scripts are written to test against the `std_capabilities` package. Make sure that the `std_capabilities` package is copied along with the `capabilities2` package to the `capabilities2_ws/src` folder and is built and sourced before running the test scripts. +Capabilities features can be tested using the provided test scripts. The test scripts primarily test the capabilities2 server using service clients. Launch the capabilities2 server before running the test scripts using -> **Note**: The test scripts use the `bondby` package. Make sure to install the `bondpy` package before running the test scripts. -> sudo apt install ros-$ROS_DISTRO-bondy +```bash +source install/setup.bash +ros2 launch capabilities2_server server.launch.py +``` + +The test scripts are written to test against the `std_capabilities` package. Make sure that the `std_capabilities` package is copied along with the `capabilities2` package to the `capabilities2_ws/src` folder and is built and sourced before running the test scripts. + +> **Note**: The test scripts use the `bondby` package. Make sure to install the `bondpy` package before running the test scripts. \ +> `sudo apt install ros-$ROS_DISTRO-bondy` Run the tests with python3. The test scripts are located in the `capabilities2_server/test` directory. Make sure to source the workspace before running the test. ```bash # example +source install/setup.bash +cd src/capabilities2/capabilities2_server/test python3 call_establish_bond.py ``` @@ -23,6 +32,13 @@ python3 call_establish_bond.py There is another test script in the `capabilities2_launch_proxy` package that tests using a capability. +```bash +# example +source install/setup.bash +cd src/capabilities2/capabilities2_launch_proxy/test +python3 call_establish_bond.py +``` + | Test Script | Description | | --- | --- | | [call_use_launch_runner.py](../capabilities2_launch_proxy/test/call_use_launch_runner.py) | Test using a launch runner based capability. This tests the bond, use, and get running features of the capabilities server | diff --git a/docs/setup.md b/docs/setup.md new file mode 100644 index 0000000..b22ff77 --- /dev/null +++ b/docs/setup.md @@ -0,0 +1,46 @@ +## Using Capabilities package (Development and Deployment) + +The current package has been tested on Ubuntu and ROS2 Humble, Rolling and Jazzy. Complete following sections in order. + +### Base folder creation + +Create the workspace folder by running following commands in the terminal. + +```bash +mkdir -p /home/$USER/capabilities_ws/src +cd /home/$USER/capabilities_ws/src +``` + +### Cloning the Packages + +Clone the package using Git + +```bash +git clone https://github.com/CollaborativeRoboticsLab/capabilities2.git +``` + +Optionally you can clone + +```bash +git https://github.com/CollaborativeRoboticsLab/std_capabilities.git +``` + +### Dependency installation + +Move the terminal to workspace root and install dependencies. + +```bash +cd /home/$USER/capabilities_ws +``` + +```bash +rosdep install --from-paths src --ignore-src -r -y +``` + +### Compile + +Use colcon to build the packages: + +```bash +colcon build +``` diff --git a/docs/setup_with_dev.md b/docs/setup_with_dev.md new file mode 100644 index 0000000..2944add --- /dev/null +++ b/docs/setup_with_dev.md @@ -0,0 +1,53 @@ +## Using Capabilities package (Development and Deployment) + +The current package has been tested on Ubuntu and ROS2 Humble, Rolling and Jazzy. Complete following sections in order. + +### Base folder creation + +Create the workspace folder by running following commands in the terminal. + +```bash +mkdir -p /home/$USER/capabilities_ws/src +cd /home/$USER/capabilities_ws/src +``` + +### Cloning the Package + +Clone the package using Git + +```bash +git clone -b develop https://github.com/CollaborativeRoboticsLab/capabilities2.git +git clone -b develop https://github.com/CollaborativeRoboticsLab/std_capabilities.git +``` + +### Devcontainer + +A `devcontainer` is provided for developing the capabilities2 meta-package. The container can be used with [Microsoft Visual Studio Code](https://code.visualstudio.com/) and [Remote Development Extention](https://marketplace.visualstudio.com/items?itemName=ms-vscode-remote.vscode-remote-extensionpack). + +If there are multiple packages in the workspace as with the usual case, move the `.devcontainer` folder into the parent folder such that it is located as follows, + +```txt +colcon_ws +--> build +--> install +--> log +--> src + --> .devcontainer + --> capabilities2 + --> pakcage .. +``` + +Close and reopen the vs code editor so that devcontainer is automatically detected. After that, Dependencies need to be installed in the container. Install the dependencies with rosdep: + +```bash +# in the devcontainer +rosdep install --from-paths src --ignore-src -r -y +``` + +### Compile + +Use colcon to build the packages: + +```bash +colcon build +``` diff --git a/readme.md b/readme.md index 91714cd..125fe00 100644 --- a/readme.md +++ b/readme.md @@ -1,14 +1,15 @@ -# capabilities2 +# Capabilities2 +![ROS](https://img.shields.io/badge/Framework-ROS2-informational?&logo=ROS) [![ROS2 Jazzy](https://img.shields.io/badge/ROS2-Jazzy-blue)](https://index.ros.org/doc/ros2/Releases/) -[![License: MIT](https://img.shields.io/badge/License-MIT-yellow.svg)](https://opensource.org/licenses/MIT) -[![Open in Visual Studio Code](https://img.shields.io/badge/vscode-dev-blue)](https://open.vscode.dev/airesearchlab/capabilities2) ![C++](https://img.shields.io/badge/Code-C++-informational?&logo=c%2b%2b) -![ROS](https://img.shields.io/badge/Framework-ROS-informational?&logo=ROS) +[![Open in Visual Studio Code](https://img.shields.io/badge/vscode-dev-blue)](https://open.vscode.dev/collaborativeroboticslab/capabilities2) +[![License: MIT](https://img.shields.io/badge/License-MIT-yellow.svg)](https://opensource.org/licenses/MIT) -A reimplementation of the [capabilities](https://github.com/osrf/capabilities) package. This package is implemented using C++17 and extends the capabilities package features. See the [capabilities2_server](./capabilities2_server/readme.md) package for the main system component. +A reimplementation of the [capabilities](https://github.com/osrf/capabilities) package. This package is implemented using `C++` and extends the capabilities package features. -[More Information about Motivation and Example Use Cases](./docs/motivation_and_examples.md) +- [capabilities2_server](./capabilities2_server/readme.md) package contains the core of the system. +- [capabilities2_runner](./capabilities2_server/readme.md) package contains base and template classes for capability implementations. ## System structure @@ -16,7 +17,7 @@ A reimplementation of the [capabilities](https://github.com/osrf/capabilities) p ## Entities -The main usage of `capabilities2` will typically involve creating capabilities through providers, interfaces and semantic interfaces. Which are represented in specification file stored as YAML, see the definitions and examples for each: +The main usage of `capabilities2` will typically involve creating or customizing capabilities through providers, interfaces and semantic interfaces. These are stored as YAML, and for more information about definitions and examples, click the links: | Entity | Description | | --- | --- | @@ -24,88 +25,15 @@ The main usage of `capabilities2` will typically involve creating capabilities t | [Providers](./docs/providers.md) | The capability provider specification file provides a mechanism to operate the capability | | [Semantic Interfaces](./docs/semantic_interfaces.md) | The semantic interface specification file provides a mechanism to redefine a capability with semantic information | -See [docs](./docs/) for the format of these entities or click the links above. Runners can be created using the runner API parent classes [here](./capabilities2_runner/readme.md). The capabilities service can be started using the [capabilities2_server](./capabilities2_server/readme.md) package. - -## Design - -The new capabilities package is designed to be more efficient and extensible. The functions are implemented as plugins, which can be loaded at runtime. The execution of providers is abstracted using an API called runners. The runners can manage more arbitrary provider operation which can include performing actions or services, or even running other capabilities. The capability models are stored in a database, This will allow various feature improvements such as hot reloading, state persistence, and model extension. Another possible feature is to create more complex ontological relationships between capabilities such as prerequisites, conflicts, or RDF triples (for example, `grasp` results in `holding`, or `pick` is a type of `manipulation`, or `grasp` is incompatible with `push`). - -### Bonds - -The bonds feature as implemented in `capabilities` and reimplemented in `capabilities2` allows applications to overlap functions by requesting use of the same resources. When all references to a resource are released, the resource is stopped. This has the added benefit of creating an idle state for the robot in which the robot computing resources are not being used. As a result of this, the robot could be more energy-efficient, and the robot could be more responsive to new tasks. - -### Runners - -The runners feature which is new in `capabilities2` allows capabilities to be executed in a more diverse manner. This could include running actions, services, or even other capabilities. This allows capabilities to be used like actions and not just started and stopped by the capabilities service. The Runner API is designed to be extensible, so that specific runners can be created on a per provider basis. Another feature that can be implemented is cross-runner communication, which could allow capabilities to be combined at runtime to perform more complex tasks. This could overcome limitations in robot programming in which there is a common need for extensive pre-definition of tasks. - -#### launch runner - -To enable the launch functionality from `capabilities` in `capabilities2`, a `launch runner` has been implemented. Due to the design of the launch system in ROS2, it was necessary to create a `launch_proxy` node which uses the `python` launch API to start and stop launch files. The runner allows uses an action to start and stop launch files, and keep track of running launch files. - - -## Using Capabilities package (Development and Deployment) - -The current package has been tested on Ubuntu and ROS2 Rolling and Jazzy. - -### Basic setup - -Create the workspace folder by running following commands in the terminal. - -```bash -mkdir -p /home/$USER/capabilities_ws/src -cd /home/$USER/capabilities_ws/src -``` - -Unzip and copy the `capabilities2` folder into the `/home/$USER/capabilities_ws/src` folder. (Final path should be like `/home/$USER/capabilities_ws/src/capabilities2`) - -### When using devcontainer - -A `devcontainer` is provided for developing the capabilities2 meta-package. The container can be used with [Microsoft Visual Studio Code](https://code.visualstudio.com/) and [Remote Development Extention](https://marketplace.visualstudio.com/items?itemName=ms-vscode-remote.vscode-remote-extensionpack). Dependencies need to be installed in the container. Install the dependencies with rosdep: - -```bash -# in the devcontainer -rosdep install --from-paths src --ignore-src -r -y -``` - -### When using without the devcontainer - -On the terminal run the following command to identify the $USER and note down the value - -```bash -echo $USER -``` - -Then open the `/home/$USER/ROS/capabilities_ws/src/capabilities2/capabilities2_server/config/capabilities.yaml` with an available text editor. Either gedit or nano can be used. - -```sh -nano /home/$USER/ROS/capabilities_ws/src/capabilities2/capabilities2_server/config/capabilities.yaml -``` - -In the opened file, replace $USER value in lines 9 & 10 with above identified value for $USER. if the $USER is ubuntu, those lines should be - -```yaml - - /home/ubuntu/capabilities_ws/src - - /home/ubuntu/capabilities_ws/src/capabilities2 -``` - -Save the file and close. +Runners can be created using the runner API parent classes [here](./capabilities2_runner/readme.md). The capabilities service can be started using the [capabilities2_server](./capabilities2_server/readme.md) package. -Move the terminal to workspace root and install dependencies. +## Setup Information -```bash -cd /home/$USER/capabilities_ws -``` -```bash -rosdep install --from-paths src --ignore-src -r -y -``` - -### Building - -Use Colcon to build the package: +- [Installation](./docs/setup.md) +- [Setup Instructions with devcontainer](./docs/setup_with_dev.md) +- [Dependency installation for Foxglove-studio](./docs/foxglove_studio.md) -```bash -colcon build -``` +## Quick Startup information ### Starting the Capabilities2 server @@ -114,13 +42,13 @@ source install/setup.bash ros2 launch capabilities2_server capabilities2_server.launch.py ``` -### Terminal based bond creation - -Read more about this [here](../capabilities2/capabilities2_server/readme.md) +## Additional Information -### Running Test cases - -Read more about this [here](../capabilities2/docs/run_test_scripts.md) +- [Motivation and Example Use Cases](./docs/motivation_and_examples.md) +- [Design Information](./docs/design.md) +- [Registering a capability](./capabilities2_server/docs/register.md) +- [Terminal based capability usage](./capabilities2_server/docs/terminal_usage.md) +- [Running test scripts](./docs/run_test_scripts.md) ## Acknowledgements @@ -129,3 +57,18 @@ This work is based on the capabilities package developed by the Open Source Robo ## Citation If you use this work in an academic context, please cite the following publication(s): + +[Capabilities2 for ROS2: Advanced Skill-Based Control for Human-Robot Interaction](https://dl.acm.org/doi/10.5555/3721488.3721623) + +```latex +@inproceedings{10.5555/3721488.3721623, + author = {Pritchard, Michael and Ratnayake, Kalana and Gamage, Buddhi and Jayasuriya, Maleen and Herath, Damith}, + title = {Capabilities2 for ROS2: Advanced Skill-Based Control for Human-Robot Interaction}, + year = {2025}, + publisher = {IEEE Press}, + booktitle = {Proceedings of the 2025 ACM/IEEE International Conference on Human-Robot Interaction}, + pages = {1067–1071}, + location = {Melbourne, Australia}, + series = {HRI '25} +} +```