From ad63ac85b7bc113aed6ba1a9209e1207fcf6b713 Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Wed, 18 Sep 2024 22:48:36 +1000 Subject: [PATCH 1/2] removed ActionClientManager and embedded its fuctionality into MultiActionRunner. Added improved trigger_action with blocking and non-blocking functionality --- .../action_client_manager.hpp | 201 ---------- .../capabilities2_runner/action_runner.hpp | 292 ++++++++------ .../capabilities2_runner/action_runnerv2.hpp | 319 +++++++++++++++ .../multi_action_runner.hpp | 339 +++++++++++++--- .../struct/action_client_bundle.hpp | 27 ++ .../listen_runner.hpp | 26 +- .../waypoint_runner.hpp | 131 ++++--- .../interfaces/nav2.yaml | 2 +- .../capabilities2_server/runner_cache.hpp | 369 +++++++++--------- 9 files changed, 1061 insertions(+), 645 deletions(-) delete mode 100644 capabilities2_runner/include/capabilities2_runner/action_client_manager.hpp create mode 100644 capabilities2_runner/include/capabilities2_runner/action_runnerv2.hpp create mode 100644 capabilities2_runner/include/capabilities2_runner/struct/action_client_bundle.hpp diff --git a/capabilities2_runner/include/capabilities2_runner/action_client_manager.hpp b/capabilities2_runner/include/capabilities2_runner/action_client_manager.hpp deleted file mode 100644 index fff84fe..0000000 --- a/capabilities2_runner/include/capabilities2_runner/action_client_manager.hpp +++ /dev/null @@ -1,201 +0,0 @@ -#pragma once - -#include -#include -#include -#include -#include - -#include -#include -#include - -namespace capabilities2_runner -{ - -template -struct ActionClientBundle -{ - std::shared_ptr> action_client; - typename rclcpp_action::Client::SendGoalOptions send_goal_options; - typename rclcpp_action::ClientGoalHandle::SharedPtr goal_handle; -}; - -/** - * @brief action manager that control multiple action clients - */ -class ActionClientManager -{ -public: - /** - * @brief Constructor of the action client manager - * - * @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 - * @param on_stopped function pointer to trigger at the termination of the action client by the server - */ - ActionClientManager(rclcpp::Node::SharedPtr node, const runner_opts& run_config, - std::function on_started = nullptr, - std::function on_terminated = nullptr, - std::function on_stopped = nullptr) - { - node_ = node; - run_config_ = run_config; - on_started_ = on_started; - on_terminated_ = on_terminated; - on_stopped_ = on_stopped; - } - - /** - * @brief Initializer function for initializing the action runner in place of constructor due to plugin semantics - * - * @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::Client::SendGoalOptions goal_options_; - typename rclcpp_action::ClientGoalHandle::SharedPtr goal_handle_; - - 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"); - } - - // send goal options - // goal response callback - goal_options_.goal_response_callback = [this, &goal_handle_](const typename rclcpp_action::ClientGoalHandle::SharedPtr& goal_handle) - { - // store goal handle to be used with stop funtion - goal_handle_ = goal_handle; - }; - - // result callback - goal_options_.result_callback = [this](const typename rclcpp_action::ClientGoalHandle::WrappedResult& wrapped_result) - { - if (wrapped_result.code == rclcpp_action::ResultCode::SUCCEEDED) - { - // publish event - if (on_started_) on_started_(run_config_.interface); - } - else - { - // send terminated event - if (on_terminated_) on_terminated_(run_config_.interface); - } - }; - - ActionClientBundle bundle{client_, goal_options_, goal_handle_}; - - action_clients_map_[action_name] = std::make_any>(bundle); - } - - /** - * @brief Deinitializer function for stopping an the action - * - * @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()); - } - } - } - - /** - * @brief Trigger function for calling an the action - * - * @param action_name action name used in the yaml file, used to load specific configuration from the run_config - */ - template - void trigger_action(const std::string& action_name, typename ActionT::Goal goal_msg) - { - auto bundle = std::any_cast>(action_clients_map_[action_name]); - - // launch runner using action client - bundle.action_client->async_send_goal(goal_msg, bundle.send_goal_options); - } - -protected: - - /** - * Dictionary to hold action client bundle. The key is a string, and the value is a polymorphic bundle. - * */ - std::map action_clients_map_; - - /** - * shared pointer to the capabilities node. Allows to use ros node related functionalities - */ - rclcpp::Node::SharedPtr node_; - - /** - * runner configuration - */ - runner_opts run_config_; - - /** - * pointer to function to execute on starting the runner - */ - std::function on_started_; - - /** - * pointer to function to execute on terminating the runner - */ - std::function on_terminated_; - - /** - * pointer to function to execute on stopping the runner - */ - std::function on_stopped_; -}; - -} // namespace capabilities2_runner diff --git a/capabilities2_runner/include/capabilities2_runner/action_runner.hpp b/capabilities2_runner/include/capabilities2_runner/action_runner.hpp index fa1e066..efc9ef9 100644 --- a/capabilities2_runner/include/capabilities2_runner/action_runner.hpp +++ b/capabilities2_runner/include/capabilities2_runner/action_runner.hpp @@ -23,131 +23,177 @@ template class ActionRunner : public RunnerBase { public: - /** - * @brief Constructor which needs to be empty due to plugin semantics - */ - ActionRunner() : RunnerBase() - { - } - - /** - * @brief Initializer function for initializing the action 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 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) - { - // initialize the runner base by storing node pointer and run config - init_base(node, run_config, on_started, on_terminated, on_stopped); - - // 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()); - - if (!action_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"); - } - - // send goal options - // goal response callback - send_goal_options_.goal_response_callback = - [this](const typename rclcpp_action::ClientGoalHandle::SharedPtr& goal_handle) { - // 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) - { - // publish event - if (on_started_) - { - on_started_(run_config_.interface); - } - } - else - { - // send terminated event - if (on_terminated_) - { - on_terminated_(run_config_.interface); - } - } - }; - } - - /** - * @brief stop function to cease functionality and shutdown - * - */ - virtual void stop() override - { - // if the node pointer is empty then throw an error - // this means that the runner was not started and is being used out of order - - if (!node_) - throw runner_exception("cannot stop runner that was not started"); - - // throw an error if the action client is null - // this can happen if the runner is not able to find the action resource - - if (!action_client_) - throw runner_exception("cannot stop runner action that was not started"); - - // stop runner using action client - if (goal_handle_) - { - try - { - auto 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); - } - }); - - // 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()); - } - } - } + /** + * @brief Constructor which needs to be empty due to plugin semantics + */ + ActionRunner() : RunnerBase() + { + } + + /** + * @brief Initializer function for initializing the action 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 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) + { + // initialize the runner base by storing node pointer and run config + init_base(node, run_config, on_started, on_terminated, on_stopped); + + // 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()); + + if (!action_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"); + } + + // 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); + } + } + }; + } + + /** + * @brief stop function to cease functionality and shutdown + * + */ + virtual void stop() override + { + // if the node pointer is empty then throw an error + // this means that the runner was not started and is being used out of order + + if (!node_) + throw runner_exception("cannot stop runner that was not started"); + + // throw an error if the action client is null + // this can happen if the runner is not able to find the action resource + + if (!action_client_) + throw runner_exception("cannot stop runner action that was not started"); + + // stop runner using action client + if (goal_handle_) + { + try + { + auto 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); + } + }); + + // 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()); + } + } + } protected: - /**< action client */ - std::shared_ptr> action_client_; - - /**< Send Goal Option struct to link result_callback, feedback_callback and goal_response_callback with action client - */ - typename rclcpp_action::Client::SendGoalOptions send_goal_options_; - - /**< goal handle parameter to capture goal response from goal_response_callback */ - typename rclcpp_action::ClientGoalHandle::SharedPtr goal_handle_; + /** + * @brief Get the name of an action resource by given action type + * + * This helps to navigate remappings from the runner config + * + * @param action_type + * @return std::string + */ + std::string get_action_name_by_type(const std::string& action_type) + { + for (const auto& resource : run_config_.resources) + { + if (resource.resource_type == "action") + { + if (resource.msg_type == action_type) + { + return resource.name; + } + } + } + + throw runner_exception("no action resource found: " + action_type); + } + + /** + * @brief get first action resource name + * + * This can be used to get the name of the first action resource in the runner config + * + * @return std::string + */ + std::string get_first_action_name() + { + for (const auto& resource : run_config_.resources) + { + if (resource.resource_type == "action") + { + return resource.name; + } + } + + throw runner_exception("no action resources found for interface: " + run_config_.interface); + } + + /**< action client */ + std::shared_ptr> action_client_; + + /**< Send Goal Option struct to link result_callback, feedback_callback and goal_response_callback with action client + */ + typename rclcpp_action::Client::SendGoalOptions send_goal_options_; + + /**< goal handle parameter to capture goal response from goal_response_callback */ + typename rclcpp_action::ClientGoalHandle::SharedPtr goal_handle_; }; } // namespace capabilities2_runner diff --git a/capabilities2_runner/include/capabilities2_runner/action_runnerv2.hpp b/capabilities2_runner/include/capabilities2_runner/action_runnerv2.hpp new file mode 100644 index 0000000..96a3efd --- /dev/null +++ b/capabilities2_runner/include/capabilities2_runner/action_runnerv2.hpp @@ -0,0 +1,319 @@ +#pragma once + +#include +#include +#include +#include + +#include +#include +#include + +#include + +namespace capabilities2_runner +{ + +/** + * @brief action runner base class + * + * Create an action client to run an action based capability + */ +template +class ActionRunner : public RunnerBase +{ +public: + /** + * @brief Constructor which needs to be empty due to plugin semantics + */ + ActionRunner() : RunnerBase() + { + } + + /** + * @brief Initializer function for initializing the action 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 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_runner(rclcpp::Node::SharedPtr node, const runner_opts& run_config, + std::function on_started = nullptr, + std::function on_terminated = nullptr, + std::function on_stopped = nullptr) + { + // initialize the runner base by storing node pointer and run config + init_base(node, run_config, on_started, on_terminated, on_stopped); + } + + /** + * @brief Initializer function for initializing the action runner in place of constructor due to plugin semantics + * + * @param action_name action name used in the yaml file, used to load specific configuration from the run_config + */ + virtual void init_action(const std::string& action_name) + { + // 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()); + + if (!action_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"); + } + + // // 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); + // } + // } + // }; + } + + /** + * @brief deinitializer function to cease functionality and shutdown + * + */ + virtual void deinit_action() + { + // 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 (!action_client_) + throw runner_exception("cannot stop runner action that was not started"); + + // stop runner using action client + if (goal_handle_) + { + try + { + auto 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); + } + }); + + // 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()); + } + } + } + + /** + * @brief Trigger function for calling and triggering an action. Non blocking implementation so result will not + * be returned. + * + * @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. + */ + bool trigger_action(typename ActionT::Goal& goal_msg) + { + // result callback + 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 = action_client_->async_send_goal(goal_msg, 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 + + goal_handle_ = goal_handle; + return true; + } + } + + /** + * @brief Trigger function for calling and triggering an action. Blocking implementation so result will be returned. + * + * @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. + */ + bool trigger_action_wait(typename ActionT::Goal& goal_msg, typename ActionT::Result::SharedPtr result_msg) + { + auto goal_handle_future = action_client_->async_send_goal(goal_msg, 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 + goal_handle_ = goal_handle; + } + + // Wait for the server to be done with the goal + auto result_future = 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: + /** + * @brief Get the name of an action resource by given action type + * + * This helps to navigate remappings from the runner config + * + * @param action_type + * @return std::string + */ + std::string get_action_name_by_type(const std::string& action_type) + { + for (const auto& resource : run_config_.resources) + { + if (resource.resource_type == "action") + { + if (resource.msg_type == action_type) + { + return resource.name; + } + } + } + + throw runner_exception("no action resource found: " + action_type); + } + + /** + * @brief get first action resource name + * + * This can be used to get the name of the first action resource in the runner config + * + * @return std::string + */ + std::string get_first_action_name() + { + for (const auto& resource : run_config_.resources) + { + if (resource.resource_type == "action") + { + return resource.name; + } + } + + throw runner_exception("no action resources found for interface: " + run_config_.interface); + } + + /**< action client */ + std::shared_ptr> action_client_; + + /**< Send Goal Option struct to link result_callback, feedback_callback and goal_response_callback with action client + */ + typename rclcpp_action::Client::SendGoalOptions send_goal_options_; + + /**< goal handle parameter to capture goal response from goal_response_callback */ + typename rclcpp_action::ClientGoalHandle::SharedPtr goal_handle_; +}; + +} // 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 index 3a913cb..d8557b1 100644 --- a/capabilities2_runner/include/capabilities2_runner/multi_action_runner.hpp +++ b/capabilities2_runner/include/capabilities2_runner/multi_action_runner.hpp @@ -11,7 +11,7 @@ #include #include -#include +#include namespace capabilities2_runner { @@ -24,71 +24,284 @@ namespace capabilities2_runner class MultiActionRunner : public RunnerBase { public: - /** - * @brief Constructor which needs to be empty due to plugin semantics - */ - MultiActionRunner() : RunnerBase() - { - } - - /** - * @brief Initializer function for initializing the action 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 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 init_runner(rclcpp::Node::SharedPtr node, const runner_opts& run_config, - std::function on_started = nullptr, - std::function on_terminated = nullptr, - std::function on_stopped = nullptr) - { - // initialize the runner base by storing node pointer and run config - init_base(node, run_config, on_started, on_terminated, on_stopped); - - // initialize the action client manager used for manageing the actions - actionClientManager_ = std::make_shared(node, run_config, on_started, on_terminated, on_stopped); - } - - /** - * @brief Initializer function for initializing the action runner in place of constructor due to plugin semantics - * - * @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) - { - actionClientManager_->init_action(action_name); - } - - /** - * @brief Deinitializer function for stopping an the action - * - * @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) - { - actionClientManager_->deinit_action(action_name); - } - - /** - * @brief Trigger function for calling an the action - * - * @param action_name action name used in the yaml file, used to load specific configuration from the run_config - */ - template - void trigger_action(const std::string& action_name, typename ActionT::Goal goal_msg) - { - actionClientManager_->trigger_action(action_name, goal_msg); - } + /** + * @brief Constructor which needs to be empty due to plugin semantics + */ + MultiActionRunner() : RunnerBase() + { + } + + /** + * @brief Initializer function for initializing the action 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 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_runner(rclcpp::Node::SharedPtr node, const runner_opts& run_config, + std::function on_started = nullptr, + std::function on_terminated = nullptr, + std::function on_stopped = nullptr) + { + // initialize the runner base by storing node pointer and run config + init_base(node, run_config, on_started, on_terminated, on_stopped); + } + + /** + * @brief Initializer function for initializing the action runner in place of constructor due to plugin semantics + * + * @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 + * + * @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()); + } + } + } + + /** + * @brief Trigger function for calling and triggering an action. Non blocking implementation so result will not + * be returned. + * + * @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. + * + * @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: + /** + * @brief Get the name of an action resource by given action type + * + * This helps to navigate remappings from the runner config + * + * @param action_type + * @return std::string + */ + std::string get_action_name_by_type(const std::string& action_type) + { + for (const auto& resource : run_config_.resources) + { + if (resource.resource_type == "action") + { + if (resource.msg_type == action_type) + { + return resource.name; + } + } + } + + throw runner_exception("no action resource found: " + action_type); + } + + /** + * @brief get first action resource name + * + * This can be used to get the name of the first action resource in the runner config + * + * @return std::string + */ + std::string get_first_action_name() + { + for (const auto& resource : run_config_.resources) + { + if (resource.resource_type == "action") + { + return resource.name; + } + } + + throw runner_exception("no action resources found for interface: " + run_config_.interface); + } - /** Action Client Manager for handling multiple actions*/ - std::shared_ptr actionClientManager_; - + /** + * 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/struct/action_client_bundle.hpp b/capabilities2_runner/include/capabilities2_runner/struct/action_client_bundle.hpp new file mode 100644 index 0000000..1042d0b --- /dev/null +++ b/capabilities2_runner/include/capabilities2_runner/struct/action_client_bundle.hpp @@ -0,0 +1,27 @@ +#pragma once + +#include +#include +#include +#include +#include + +#include +#include +#include + +namespace capabilities2_runner +{ + +/** + * @brief templated struct to handle Action clients and their respective goal_handles + */ +template +struct ActionClientBundle +{ + std::shared_ptr> action_client; + typename rclcpp_action::ClientGoalHandle::SharedPtr goal_handle; + typename rclcpp_action::Client::SendGoalOptions send_goal_options; +}; + +} // namespace capabilities2_runner \ No newline at end of file diff --git a/capabilities2_runner_audio/include/capabilities2_runner_audio/listen_runner.hpp b/capabilities2_runner_audio/include/capabilities2_runner_audio/listen_runner.hpp index f04037f..dbf7b3c 100644 --- a/capabilities2_runner_audio/include/capabilities2_runner_audio/listen_runner.hpp +++ b/capabilities2_runner_audio/include/capabilities2_runner_audio/listen_runner.hpp @@ -1,12 +1,14 @@ #pragma once #include -#include -#include + #include #include #include +#include +#include + namespace capabilities2_runner { @@ -41,14 +43,14 @@ class VoiceListenerRunner : public MultiActionRunner init_action("speech_to_text"); } - /** - * @brief stop function to cease functionality and shutdown - * - */ - virtual void stop() override - { + /** + * @brief stop function to cease functionality and shutdown + * + */ + virtual void stop() override + { deinit_action("speech_to_text"); - } + } /** * @brief trigger the runner @@ -58,15 +60,13 @@ class VoiceListenerRunner : public MultiActionRunner virtual void trigger(std::shared_ptr parameters = nullptr) { 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(); - trigger_action("speech_to_text", goal_msg); - + bool result = trigger_action_wait("speech_to_text", goal_msg, result_msg); } - - protected: std::string global_frame_; /**The global frame of the robot*/ std::string robot_base_frame_; /**The frame of the robot base*/ diff --git a/capabilities2_runner_nav2/include/capabilities2_runner_nav2/waypoint_runner.hpp b/capabilities2_runner_nav2/include/capabilities2_runner_nav2/waypoint_runner.hpp index d9f0a48..a0236a3 100644 --- a/capabilities2_runner_nav2/include/capabilities2_runner_nav2/waypoint_runner.hpp +++ b/capabilities2_runner_nav2/include/capabilities2_runner_nav2/waypoint_runner.hpp @@ -1,12 +1,15 @@ #pragma once +#include + #include -#include -#include -#include #include #include -#include + +#include +#include + +#include namespace capabilities2_runner { @@ -20,61 +23,71 @@ namespace capabilities2_runner 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 void 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); - - // launch runner using action client - action_client_->async_send_goal(goal_msg, send_goal_options_); - } - - 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*/ + 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_runner(node, run_config, on_started, on_terminated, on_stopped); + + init_action("follow_waypoints"); + } + + /** + * @brief stop function to cease functionality and shutdown + * + */ + virtual void stop() override + { + deinit_action(); + } + + /** + * @brief trigger the runner + * + @param parameters XMLElement that contains parameters in the format '' + */ + virtual void 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); + + bool success = trigger_action(goal_msg); + } + +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 index 2ffe886..f0dd6aa 100644 --- a/capabilities2_runner_nav2/interfaces/nav2.yaml +++ b/capabilities2_runner_nav2/interfaces/nav2.yaml @@ -7,7 +7,7 @@ spec_type: interface description: "Navigational capabilities using Nav2 stack" interface: actions: - "WaypointFollower": + "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 diff --git a/capabilities2_server/include/capabilities2_server/runner_cache.hpp b/capabilities2_server/include/capabilities2_server/runner_cache.hpp index 0f6f644..8648887 100644 --- a/capabilities2_server/include/capabilities2_server/runner_cache.hpp +++ b/capabilities2_server/include/capabilities2_server/runner_cache.hpp @@ -25,191 +25,190 @@ namespace capabilities2_server class RunnerCache { public: - RunnerCache() : runner_loader_("capabilities2_runner", "capabilities2_runner::RunnerBase") - { - on_started = nullptr; - on_stopped = nullptr; - on_terminated = nullptr; - } - - /** - * @brief Add a runner to the cache - * - * @param node pointer to the origin node, generally the capabilities2_server - * @param capability capability name to be loaded - * @param run_config run_config of the runner to be loaded - * @param parameters parameters related to the runner in std::string form for compatibility accross various runners - */ - void add_runner(rclcpp::Node::SharedPtr node, const std::string& capability, - const models::run_config_model_t& run_config, - std::shared_ptr parameters = nullptr) - { - // if the runner exists then throw an error - if (running(capability)) - { - // already running - throw capabilities2_runner::runner_exception("capability is running already: " + capability); - // return; - } - - // check if run config is valid - if (!run_config.is_valid()) - { - 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 - if (run_config.runner.find(".launch") != std::string::npos || - run_config.runner.find("/") != std::string::npos || - run_config.runner.find(".py") != std::string::npos) - { - runner_cache_[capability] = runner_loader_.createSharedInstance("capabilities2_runner::LaunchRunner"); - } - 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); - } - - /** - * @brief Trigger a runner in the cache - * - * @param node pointer to the origin node, generally the capabilities2_server - * @param capability capability name to be loaded - * @param parameters parameters related to the runner in std::string form for compatibility accross various runners - */ - void trigger_runner(const std::string& capability, std::shared_ptr parameters = nullptr) - { - runner_cache_[capability]->trigger(parameters); - } - - /** - * @brief Remove a given runner - * - * @param capability capability to be removed - */ - void remove_runner(const std::string& capability) - { - // find the runner in the cache - if (runner_cache_.find(capability) == runner_cache_.end()) - { - // 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(); - } - catch (const capabilities2_runner::runner_exception& e) - { - // pass - } - - // reset the runner pointer - runner_cache_[capability].reset(); - - // remove the runner from map - runner_cache_.erase(capability); - } - - /** - * @brief Get a list of active runners in the cache - * - * @return const std::vector of runners - */ - const std::vector get_running_capabilities() - { - std::vector runners; - for (const auto& runner : runner_cache_) - { - runners.push_back(runner.first); - } - return runners; - } - - /** - * @brief Get provider of capability - * - * @param capability capability of which the provider is requested - * @return provider name - */ - const std::string provider(const std::string& capability) - { - return runner_cache_[capability]->get_provider(); - } - - /** - * @brief get started_by of capability - * - * @param capability capability of which the started_by is requested - * @return name of the started_by - */ - const std::string started_by(const std::string& capability) - { - return runner_cache_[capability]->get_started_by(); - } - - /** - * @brief get pid of capability - * - * @param capability capability of which the pid is requested - * @return value of pid - */ - const std::string pid(const std::string& capability) - { - return runner_cache_[capability]->get_pid(); - } - - /** - * @brief Check if a capability has a runner - * - * @param capability - * @return true - * @return false - */ - bool running(const std::string& capability) - { - 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; - } + RunnerCache() : runner_loader_("capabilities2_runner", "capabilities2_runner::RunnerBase") + { + on_started = nullptr; + on_stopped = nullptr; + on_terminated = nullptr; + } + + /** + * @brief Add a runner to the cache + * + * @param node pointer to the origin node, generally the capabilities2_server + * @param capability capability name to be loaded + * @param run_config run_config of the runner to be loaded + * @param parameters parameters related to the runner in std::string form for compatibility accross various runners + */ + void add_runner(rclcpp::Node::SharedPtr node, const std::string& capability, + const models::run_config_model_t& run_config, + std::shared_ptr parameters = nullptr) + { + // if the runner exists then throw an error + if (running(capability)) + { + // already running + throw capabilities2_runner::runner_exception("capability is running already: " + capability); + // return; + } + + // check if run config is valid + if (!run_config.is_valid()) + { + 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 + if (run_config.runner.find(".launch") != std::string::npos || run_config.runner.find("/") != std::string::npos || + run_config.runner.find(".py") != std::string::npos) + { + runner_cache_[capability] = runner_loader_.createSharedInstance("capabilities2_runner::LaunchRunner"); + } + 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); + } + + /** + * @brief Trigger a runner in the cache + * + * @param node pointer to the origin node, generally the capabilities2_server + * @param capability capability name to be loaded + * @param parameters parameters related to the runner in std::string form for compatibility accross various runners + */ + void trigger_runner(const std::string& capability, std::shared_ptr parameters = nullptr) + { + runner_cache_[capability]->trigger(parameters); + } + + /** + * @brief Remove a given runner + * + * @param capability capability to be removed + */ + void remove_runner(const std::string& capability) + { + // find the runner in the cache + if (runner_cache_.find(capability) == runner_cache_.end()) + { + // 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(); + } + catch (const capabilities2_runner::runner_exception& e) + { + // pass + } + + // reset the runner pointer + runner_cache_[capability].reset(); + + // remove the runner from map + runner_cache_.erase(capability); + } + + /** + * @brief Get a list of active runners in the cache + * + * @return const std::vector of runners + */ + const std::vector get_running_capabilities() + { + std::vector runners; + for (const auto& runner : runner_cache_) + { + runners.push_back(runner.first); + } + return runners; + } + + /** + * @brief Get provider of capability + * + * @param capability capability of which the provider is requested + * @return provider name + */ + const std::string provider(const std::string& capability) + { + return runner_cache_[capability]->get_provider(); + } + + /** + * @brief get started_by of capability + * + * @param capability capability of which the started_by is requested + * @return name of the started_by + */ + const std::string started_by(const std::string& capability) + { + return runner_cache_[capability]->get_started_by(); + } + + /** + * @brief get pid of capability + * + * @param capability capability of which the pid is requested + * @return value of pid + */ + const std::string pid(const std::string& capability) + { + return runner_cache_[capability]->get_pid(); + } + + /** + * @brief Check if a capability has a runner + * + * @param capability + * @return true + * @return false + */ + bool running(const std::string& capability) + { + 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 From f28aab959ce4d42fde6c902292ecb333dffeb2da Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Wed, 18 Sep 2024 23:01:38 +1000 Subject: [PATCH 2/2] comment additions --- .../include/capabilities2_runner/action_runnerv2.hpp | 3 ++- .../include/capabilities2_runner/multi_action_runner.hpp | 3 ++- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/capabilities2_runner/include/capabilities2_runner/action_runnerv2.hpp b/capabilities2_runner/include/capabilities2_runner/action_runnerv2.hpp index 96a3efd..8f10f5f 100644 --- a/capabilities2_runner/include/capabilities2_runner/action_runnerv2.hpp +++ b/capabilities2_runner/include/capabilities2_runner/action_runnerv2.hpp @@ -149,7 +149,7 @@ class ActionRunner : public RunnerBase /** * @brief Trigger function for calling and triggering an action. Non blocking implementation so result will not - * be returned. + * be returned. Use when action triggering is required and result message of the action is not required. * * @param goal_msg goal message to be sent to the action server * @@ -200,6 +200,7 @@ class ActionRunner : public RunnerBase /** * @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 goal_msg goal message to be sent to the action server * @param result_msg result message returned by the action server upon completion diff --git a/capabilities2_runner/include/capabilities2_runner/multi_action_runner.hpp b/capabilities2_runner/include/capabilities2_runner/multi_action_runner.hpp index d8557b1..e31d18d 100644 --- a/capabilities2_runner/include/capabilities2_runner/multi_action_runner.hpp +++ b/capabilities2_runner/include/capabilities2_runner/multi_action_runner.hpp @@ -131,7 +131,7 @@ class MultiActionRunner : public RunnerBase /** * @brief Trigger function for calling and triggering an action. Non blocking implementation so result will not - * be returned. + * 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 @@ -187,6 +187,7 @@ class MultiActionRunner : public RunnerBase /** * @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