diff --git a/capabilities2_runner/include/capabilities2_runner/action_client_manager.hpp b/capabilities2_runner/include/capabilities2_runner/action_client_manager.hpp index 6cf0338..e69de29 100644 --- a/capabilities2_runner/include/capabilities2_runner/action_client_manager.hpp +++ b/capabilities2_runner/include/capabilities2_runner/action_client_manager.hpp @@ -1,202 +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 99f0ed2..efc9ef9 100644 --- a/capabilities2_runner/include/capabilities2_runner/action_runner.hpp +++ b/capabilities2_runner/include/capabilities2_runner/action_runner.hpp @@ -65,6 +65,11 @@ class ActionRunner : public RunnerBase // 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; }; @@ -74,11 +79,7 @@ class ActionRunner : public RunnerBase [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); - } + // Do something } else { @@ -140,6 +141,50 @@ class ActionRunner : public RunnerBase } 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_; 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..8f10f5f --- /dev/null +++ b/capabilities2_runner/include/capabilities2_runner/action_runnerv2.hpp @@ -0,0 +1,320 @@ +#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. 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 + * + * @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. + * 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 + * + * @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 3b0617d..e31d18d 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 { @@ -38,6 +38,7 @@ class MultiActionRunner : public RunnerBase * @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, @@ -46,10 +47,6 @@ class MultiActionRunner : public RunnerBase { // 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); } /** @@ -60,7 +57,24 @@ class MultiActionRunner : public RunnerBase template void init_action(const std::string& action_name) { - actionClientManager_->init_action(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); } /** @@ -71,23 +85,224 @@ class MultiActionRunner : public RunnerBase template void deinit_action(const std::string& action_name) { - actionClientManager_->deinit_action(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 + * @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 - void trigger_action(const std::string& action_name, typename ActionT::Goal goal_msg) + bool trigger_action(const std::string& action_name, typename ActionT::Goal& goal_msg) { - actionClientManager_->trigger_action(action_name, 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: - /** Action Client Manager for handling multiple actions*/ - std::shared_ptr actionClientManager_; + /** + * @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); + } + + /** + * 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 13b43e7..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 { @@ -58,10 +60,11 @@ 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: 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