From 25d1f48d902f3b2ad7100a78db532d8c9cdb93c1 Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Sat, 5 Oct 2024 02:52:25 +1000 Subject: [PATCH 001/133] updated runners in all packages in capabilities2_runner, capabilties2_runner_audio, and capabilities2_runner_nav2 by fixing issues and adding new std::optinals functionality from action_runner --- capabilities2_executor/.clang-format | 2 +- .../capabilities_executor.hpp | 68 +++++---------- .../capabilities2_runner/action_runner.hpp | 55 ++++++------ .../capabilities2_runner/launch_runner.hpp | 15 +++- .../notrigger_action_runner.hpp | 8 +- .../capabilities2_runner/runner_base.hpp | 32 ++++--- capabilities2_runner_audio/CMakeLists.txt | 5 ++ .../listen_runner.hpp | 64 ++++++++------ .../speak_runner.hpp | 85 +++++++++++++++++++ .../interfaces/speech_to_text.yaml | 15 ++++ .../interfaces/text_to_speech.yaml | 16 ++++ capabilities2_runner_audio/package.xml | 16 ++++ capabilities2_runner_audio/plugins.xml | 7 +- .../providers/ListenerRunner.yaml | 9 ++ .../providers/SpeakerRunner.yaml | 9 ++ .../src/audio_runners.cpp | 4 +- capabilities2_runner_nav2/CMakeLists.txt | 5 ++ .../waypoint_runner.hpp | 74 ++++++---------- .../{nav2.yaml => follow_waypoints.yaml} | 8 +- capabilities2_runner_nav2/package.xml | 10 +++ .../providers/WaypointRunner.yaml | 9 ++ .../capabilities2_server/capabilities_api.hpp | 2 +- .../capabilities2_server/runner_cache.hpp | 2 +- 23 files changed, 344 insertions(+), 176 deletions(-) create mode 100644 capabilities2_runner_audio/interfaces/speech_to_text.yaml create mode 100644 capabilities2_runner_audio/interfaces/text_to_speech.yaml create mode 100644 capabilities2_runner_audio/providers/ListenerRunner.yaml create mode 100644 capabilities2_runner_audio/providers/SpeakerRunner.yaml rename capabilities2_runner_nav2/interfaces/{nav2.yaml => follow_waypoints.yaml} (59%) create mode 100644 capabilities2_runner_nav2/providers/WaypointRunner.yaml diff --git a/capabilities2_executor/.clang-format b/capabilities2_executor/.clang-format index d36804f..92effdd 100644 --- a/capabilities2_executor/.clang-format +++ b/capabilities2_executor/.clang-format @@ -14,7 +14,7 @@ BreakBeforeBinaryOperators: false BreakBeforeTernaryOperators: false BreakConstructorInitializersBeforeComma: true BinPackParameters: true -ColumnLimit: 120 +ColumnLimit: 150 ConstructorInitializerAllOnOneLineOrOnePerLine: true DerivePointerBinding: false PointerBindsToType: true diff --git a/capabilities2_executor/include/capabilities2_executor/capabilities_executor.hpp b/capabilities2_executor/include/capabilities2_executor/capabilities_executor.hpp index 5624987..89dd936 100644 --- a/capabilities2_executor/include/capabilities2_executor/capabilities_executor.hpp +++ b/capabilities2_executor/include/capabilities2_executor/capabilities_executor.hpp @@ -32,16 +32,14 @@ class CapabilitiesExecutor : public rclcpp::Node { public: - CapabilitiesExecutor(const rclcpp::NodeOptions& options = rclcpp::NodeOptions()) - : Node("Capabilities2_Executor", options) + CapabilitiesExecutor(const rclcpp::NodeOptions& options = rclcpp::NodeOptions()) : Node("Capabilities2_Executor", options) { control_tag_list.push_back("sequential"); control_tag_list.push_back("parallel"); control_tag_list.push_back("recovery"); this->planner_server_ = rclcpp_action::create_server( - this, "~/capabilities", - std::bind(&CapabilitiesExecutor::handle_goal, this, std::placeholders::_1, std::placeholders::_2), + this, "~/capabilities", std::bind(&CapabilitiesExecutor::handle_goal, this, std::placeholders::_1, std::placeholders::_2), std::bind(&CapabilitiesExecutor::handle_cancel, this, std::placeholders::_1), std::bind(&CapabilitiesExecutor::handle_accepted, this, std::placeholders::_1)); @@ -62,8 +60,7 @@ class CapabilitiesExecutor : public rclcpp::Node * @param goal pointer to the action goal message * @return rclcpp_action::GoalResponse */ - rclcpp_action::GoalResponse handle_goal(const rclcpp_action::GoalUUID& uuid, - std::shared_ptr goal) + rclcpp_action::GoalResponse handle_goal(const rclcpp_action::GoalUUID& uuid, std::shared_ptr goal) { RCLCPP_INFO(this->get_logger(), "Received the goal request with the plan"); (void)uuid; @@ -87,8 +84,7 @@ class CapabilitiesExecutor : public rclcpp::Node * @param goal_handle pointer to the action goal handle * @return rclcpp_action::GoalResponse */ - rclcpp_action::CancelResponse - handle_cancel(const std::shared_ptr> goal_handle) + rclcpp_action::CancelResponse handle_cancel(const std::shared_ptr> goal_handle) { RCLCPP_INFO(this->get_logger(), "Received the request to cancel the plan"); (void)goal_handle; @@ -101,11 +97,9 @@ class CapabilitiesExecutor : public rclcpp::Node * * @param goal_handle pointer to the action goal handle */ - void - handle_accepted(const std::shared_ptr> goal_handle) + void handle_accepted(const std::shared_ptr> goal_handle) { - execution_thread = std::make_unique( - std::bind(&CapabilitiesExecutor::execute_plan, this, std::placeholders::_1), goal_handle); + execution_thread = std::make_unique(std::bind(&CapabilitiesExecutor::execute_plan, this, std::placeholders::_1), goal_handle); } /** @@ -142,21 +136,16 @@ class CapabilitiesExecutor : public rclcpp::Node // request semantic interface from the server auto result_semantic_future = get_sem_interf_client_->async_send_request(request_sematic); - RCLCPP_INFO(this->get_logger(), "Requesting Semantic Interface information from Capabilities2 Server for %s", - interface.c_str()); + RCLCPP_INFO(this->get_logger(), "Requesting Semantic Interface information from Capabilities2 Server for %s", interface.c_str()); // wait until data is received - if (rclcpp::spin_until_future_complete(shared_from_this(), result_semantic_future) != - rclcpp::FutureReturnCode::SUCCESS) + if (rclcpp::spin_until_future_complete(shared_from_this(), result_semantic_future) != rclcpp::FutureReturnCode::SUCCESS) { - RCLCPP_INFO(this->get_logger(), - "Failed to receive Semantic Interface information from Capabilities2 Server for %s", - interface.c_str()); + RCLCPP_INFO(this->get_logger(), "Failed to receive Semantic Interface information from Capabilities2 Server for %s", interface.c_str()); return false; } - RCLCPP_INFO(this->get_logger(), "Received Semantic Interface information from Capabilities2 Server for %s", - interface.c_str()); + RCLCPP_INFO(this->get_logger(), "Received Semantic Interface information from Capabilities2 Server for %s", interface.c_str()); // add sematic interfaces to the list if available if (result_semantic_future.get()->semantic_interfaces.size() > 0) @@ -171,21 +160,17 @@ class CapabilitiesExecutor : public rclcpp::Node auto result_providers_future = get_providers_client_->async_send_request(request_providers); - RCLCPP_INFO(this->get_logger(), - "Requesting Providers information from Capabilities2 Server for semantic interface %s", + RCLCPP_INFO(this->get_logger(), "Requesting Providers information from Capabilities2 Server for semantic interface %s", semantic_interface.c_str()); // wait until data is received - if (rclcpp::spin_until_future_complete(shared_from_this(), result_providers_future) != - rclcpp::FutureReturnCode::SUCCESS) + if (rclcpp::spin_until_future_complete(shared_from_this(), result_providers_future) != rclcpp::FutureReturnCode::SUCCESS) { - RCLCPP_INFO(this->get_logger(), "Failed to receive Providers information from Capabilities2 Server for %s", - semantic_interface.c_str()); + RCLCPP_INFO(this->get_logger(), "Failed to receive Providers information from Capabilities2 Server for %s", semantic_interface.c_str()); return false; } - RCLCPP_INFO(this->get_logger(), "Received Providers information from Capabilities2 Server for %s", - semantic_interface.c_str()); + RCLCPP_INFO(this->get_logger(), "Received Providers information from Capabilities2 Server for %s", semantic_interface.c_str()); // add defualt provider to the list providers_list.push_back(result_providers_future.get()->default_provider); @@ -211,21 +196,16 @@ class CapabilitiesExecutor : public rclcpp::Node auto result_providers_future = get_providers_client_->async_send_request(request_providers); - RCLCPP_INFO(this->get_logger(), - "Requesting Providers information from Capabilities2 Server for semantic interface %s", - interface.c_str()); + RCLCPP_INFO(this->get_logger(), "Requesting Providers information from Capabilities2 Server for semantic interface %s", interface.c_str()); // wait until data is received - if (rclcpp::spin_until_future_complete(shared_from_this(), result_providers_future) != - rclcpp::FutureReturnCode::SUCCESS) + if (rclcpp::spin_until_future_complete(shared_from_this(), result_providers_future) != rclcpp::FutureReturnCode::SUCCESS) { - RCLCPP_INFO(this->get_logger(), "Failed to receive Providers information from Capabilities2 Server for %s", - interface.c_str()); + RCLCPP_INFO(this->get_logger(), "Failed to receive Providers information from Capabilities2 Server for %s", interface.c_str()); return false; } - RCLCPP_INFO(this->get_logger(), "Received Providers information from Capabilities2 Server for %s", - interface.c_str()); + RCLCPP_INFO(this->get_logger(), "Received Providers information from Capabilities2 Server for %s", interface.c_str()); providers_list.push_back(result_providers_future.get()->default_provider); @@ -252,8 +232,7 @@ class CapabilitiesExecutor : public rclcpp::Node { // intialize a vector to accomodate elements from both std::vector tag_list(interface_list.size() + control_tag_list.size()); - std::merge(interface_list.begin(), interface_list.end(), control_tag_list.begin(), control_tag_list.end(), - tag_list.begin()); + std::merge(interface_list.begin(), interface_list.end(), control_tag_list.begin(), control_tag_list.end(), tag_list.begin()); // verify whether document got 'plan' tags if (!capabilities2_xml_parser::check_plan_tag(document)) @@ -309,8 +288,7 @@ class CapabilitiesExecutor : public rclcpp::Node * * @param server_goal_handle goal handle of the server */ - void execute_plan( - const std::shared_ptr> server_goal_handle) + void execute_plan(const std::shared_ptr> server_goal_handle) { auto result = std::make_shared(); @@ -388,8 +366,7 @@ class CapabilitiesExecutor : public rclcpp::Node // send goal options // goal response callback send_goal_options.goal_response_callback = - [this, server_goal_handle]( - const rclcpp_action::ClientGoalHandle::SharedPtr& goal_handle) { + [this, server_goal_handle](const rclcpp_action::ClientGoalHandle::SharedPtr& goal_handle) { if (!goal_handle) { RCLCPP_ERROR(this->get_logger(), "Goal was rejected by server"); @@ -410,8 +387,7 @@ class CapabilitiesExecutor : public rclcpp::Node // result callback send_goal_options.result_callback = - [this, server_goal_handle]( - const rclcpp_action::ClientGoalHandle::WrappedResult& result) { + [this, server_goal_handle](const rclcpp_action::ClientGoalHandle::WrappedResult& result) { auto result_out = std::make_shared(); switch (result.code) diff --git a/capabilities2_runner/include/capabilities2_runner/action_runner.hpp b/capabilities2_runner/include/capabilities2_runner/action_runner.hpp index e331d3d..94e440a 100644 --- a/capabilities2_runner/include/capabilities2_runner/action_runner.hpp +++ b/capabilities2_runner/include/capabilities2_runner/action_runner.hpp @@ -36,17 +36,19 @@ class ActionRunner : public RunnerBase * @param node shared pointer to the capabilities node. Allows to use ros node related functionalities * @param run_config runner configuration loaded from the yaml file * @param action_name action name used in the yaml file, used to load specific configuration from the run_config - * @param on_started function pointer to trigger at the start of the action client in the runner - * @param on_terminated function pointer to trigger at the termination of the action client in the runner - * @param on_stopped function pointer to trigger at the termination of the action client by the server + * @param on_started pointer to function to execute on starting the runner + * @param on_failure pointer to function to execute on failure of the runner + * @param on_success pointer to function to execute on success of the runner + * @param on_stopped pointer to function to execute on stopping the runner */ 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_failure = nullptr, + std::function on_success = 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); + init_base(node, run_config, on_started, on_failure, on_success, on_stopped); // create an action client action_client_ = rclcpp_action::create_client(node_, action_name); @@ -79,15 +81,15 @@ class ActionRunner : public RunnerBase [this](const typename rclcpp_action::ClientGoalHandle::WrappedResult& wrapped_result) { if (wrapped_result.code == rclcpp_action::ResultCode::SUCCEEDED) { - // Do something + // send terminated event + if (on_success_) + on_success_(run_config_.interface); } else { // send terminated event - if (on_terminated_) - { - on_terminated_(run_config_.interface); - } + if (on_failure_) + on_failure_(run_config_.interface); } }; } @@ -146,10 +148,10 @@ class ActionRunner : public RunnerBase * and then trigger the action * * @param parameters - * @return std::optional)>> + * @return std::optional> */ - virtual std::optional)>> - trigger(std::shared_ptr parameters = nullptr) override + virtual std::optional> + trigger(tinyxml2::XMLElement* parameters = nullptr) override { // the action is being triggered out of order if (!goal_handle_) @@ -173,15 +175,21 @@ class ActionRunner : public RunnerBase } // get result future - typename rclcpp_action::ClientGoalHandle::SharedPtr goal_handle; + 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 std::nullopt; + } + auto result_future = action_client_->async_get_result(goal_handle); // create a function to call for the result // the future will be returned to the caller // and the caller can provide a conversion function // to handle the result - std::function)> result_callback = - [this, result_future](std::shared_ptr result) { + std::function result_callback = + [this, result_future](tinyxml2::XMLElement* result) { // wait for result if (rclcpp::spin_until_future_complete(node_, result_future) != rclcpp::FutureReturnCode::SUCCESS) { @@ -195,12 +203,6 @@ class ActionRunner : public RunnerBase // convert the result if (wrapped_result.code == rclcpp_action::ResultCode::SUCCEEDED) { - // publish event - if (on_result_) - { - on_result_(run_config_.interface); - } - result = generate_result(wrapped_result.result); return; } @@ -221,7 +223,7 @@ class ActionRunner : public RunnerBase * @param parameters * @return ActionT::Goal the generated goal */ - virtual typename ActionT::Goal generate_goal(std::shared_ptr parameters) = 0; + virtual typename ActionT::Goal generate_goal(tinyxml2::XMLElement* parameters) = 0; /** * @brief generate a typed erased goal result @@ -232,9 +234,12 @@ class ActionRunner : public RunnerBase * The pattern needs to be implemented in the derived class * * @param wrapped_result - * @return std::shared_ptr + * @return tinyxml2::XMLElement* */ - virtual std::shared_ptr generate_result(const typename ActionT::Result::SharedPtr& result) = 0; + virtual tinyxml2::XMLElement* generate_result(const typename ActionT::Result::SharedPtr& result) = 0; + + /**< pointer to XMLElement which contain parameters */ + tinyxml2::XMLElement* parameters_; /**< action client */ typename rclcpp_action::Client::SharedPtr action_client_; diff --git a/capabilities2_runner/include/capabilities2_runner/launch_runner.hpp b/capabilities2_runner/include/capabilities2_runner/launch_runner.hpp index c4c7129..5faa18f 100644 --- a/capabilities2_runner/include/capabilities2_runner/launch_runner.hpp +++ b/capabilities2_runner/include/capabilities2_runner/launch_runner.hpp @@ -21,13 +21,24 @@ class LaunchRunner : public NoTriggerActionRunner on_started = nullptr, - std::function on_terminated = nullptr, + std::function on_failure = nullptr, + std::function on_success = nullptr, std::function on_stopped = nullptr) override { // store node pointer and run_config - init_action(node, run_config, "capabilities_launch_proxy/launch", on_started, on_terminated, on_stopped); + init_action(node, run_config, "capabilities_launch_proxy/launch", on_started, on_failure, on_success, on_stopped); // get the package path from environment variable std::string package_path; diff --git a/capabilities2_runner/include/capabilities2_runner/notrigger_action_runner.hpp b/capabilities2_runner/include/capabilities2_runner/notrigger_action_runner.hpp index edc07eb..0dc8a8b 100644 --- a/capabilities2_runner/include/capabilities2_runner/notrigger_action_runner.hpp +++ b/capabilities2_runner/include/capabilities2_runner/notrigger_action_runner.hpp @@ -18,20 +18,20 @@ class NoTriggerActionRunner : public ActionRunner { public: // throw on trigger function - std::optional)>> - trigger(std::shared_ptr parameters) override + std::optional> + trigger(tinyxml2::XMLElement* parameters) override { throw runner_exception("cannot trigger this is a no-trigger action runner"); } protected: // throw on xml conversion functions - typename ActionT::Goal generate_goal(std::shared_ptr) override + typename ActionT::Goal generate_goal(tinyxml2::XMLElement*) override { throw runner_exception("cannot generate goal this is a no-trigger action runner"); } - std::shared_ptr generate_result(const typename ActionT::Result::SharedPtr& result) override + tinyxml2::XMLElement* generate_result(const typename ActionT::Result::SharedPtr& result) override { throw runner_exception("cannot generate result this is a no-trigger action runner"); } diff --git a/capabilities2_runner/include/capabilities2_runner/runner_base.hpp b/capabilities2_runner/include/capabilities2_runner/runner_base.hpp index 1c12a84..a7eaf1c 100644 --- a/capabilities2_runner/include/capabilities2_runner/runner_base.hpp +++ b/capabilities2_runner/include/capabilities2_runner/runner_base.hpp @@ -77,13 +77,15 @@ class RunnerBase * @param node shared pointer to the capabilities node. Allows to use ros node related functionalities * @param run_config runner configuration loaded from the yaml file * @param on_started pointer to function to execute on starting the runner - * @param on_terminated pointer to function to execute on terminating the runner + * @param on_failure pointer to function to execute on failure of the runner + * @param on_success pointer to function to execute on success of the runner * @param on_stopped pointer to function to execute on stopping 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) = 0; + std::function on_started = nullptr, + std::function on_failure = nullptr, + std::function on_success = nullptr, + std::function on_stopped = nullptr) = 0; /** * @brief stop the runner @@ -102,8 +104,8 @@ class RunnerBase * @return std::optional)>> function pointer to invoke * elsewhere such as an event callback */ - virtual std::optional)>> - trigger(std::shared_ptr parameters = nullptr) = 0; + virtual std::optional> + trigger(tinyxml2::XMLElement* parameters = nullptr) = 0; /** * @brief Initializer function for initializing the base runner in place of constructor due to plugin semantics @@ -115,15 +117,17 @@ class RunnerBase */ void init_base(rclcpp::Node::SharedPtr node, const runner_opts& run_config, std::function on_started = nullptr, - std::function on_terminated = nullptr, + std::function on_failure = nullptr, + std::function on_success = nullptr, std::function on_stopped = nullptr) { // store node pointer and opts node_ = node; run_config_ = run_config; on_started_ = on_started; - on_terminated_ = on_terminated; + on_failure_ = on_failure; on_stopped_ = on_stopped; + on_success_ = on_success; } /** @@ -317,19 +321,19 @@ class RunnerBase std::function on_started_; /** - * @param node pointer to function to execute on terminating the runner + * @param node pointer to function to execute on success */ - std::function on_terminated_; + std::function on_success_; /** - * @param node pointer to function to execute on stopping the runner + * @param node pointer to function to execute on failure */ - std::function on_stopped_; + std::function on_failure_; /** - * @param node pointer to function to execute on result + * @param node pointer to function to execute on stopping the runner */ - std::function on_result_; + std::function on_stopped_; }; } // namespace capabilities2_runner diff --git a/capabilities2_runner_audio/CMakeLists.txt b/capabilities2_runner_audio/CMakeLists.txt index 15ba918..b2a54bd 100644 --- a/capabilities2_runner_audio/CMakeLists.txt +++ b/capabilities2_runner_audio/CMakeLists.txt @@ -56,6 +56,11 @@ install(DIRECTORY include/ DESTINATION include ) +install(DIRECTORY interfaces + DESTINATION share/${PROJECT_NAME} +) + + ament_export_include_directories(include) ament_export_libraries(${PROJECT_NAME}) 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 0df18c1..e8f1f61 100644 --- a/capabilities2_runner_audio/include/capabilities2_runner_audio/listen_runner.hpp +++ b/capabilities2_runner_audio/include/capabilities2_runner_audio/listen_runner.hpp @@ -1,13 +1,14 @@ #pragma once +#include #include #include #include -#include #include -#include + +#include namespace capabilities2_runner { @@ -15,13 +16,12 @@ namespace capabilities2_runner /** * @brief action runner base class * - * Class to run VoiceListener action based capability - * + * Class to run speech_to_text action based capability */ -class VoiceListenerRunner : public MultiActionRunner +class ListenerRunner : public ActionRunner { public: - VoiceListenerRunner() : MultiActionRunner() + ListenerRunner() : ActionRunner() { } @@ -30,44 +30,54 @@ class VoiceListenerRunner : public MultiActionRunner * * @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_started pointer to function to execute on starting the runner + * @param on_failure pointer to function to execute on failure of the runner + * @param on_success pointer to function to execute on success of the runner + * @param on_stopped pointer to function to execute on stopping 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_failure = nullptr, + std::function on_success = nullptr, std::function on_stopped = nullptr) override { - init_base(node, run_config, on_started, on_terminated, on_stopped); - - init_action("speech_to_text"); + init_action(node, run_config, "speech_to_text", on_started, on_failure, on_success, on_stopped); } +protected: /** - * @brief stop function to cease functionality and shutdown - * + * @brief This generate goal function overrides the generate_goal() function from ActionRunner() + * + * @param parameters XMLElement that contains parameters in the format '' + * @return ActionT::Goal the generated goal */ - virtual void stop() override + virtual hri_audio_msgs::action::SpeechToText::Goal generate_goal(tinyxml2::XMLElement* parameters) override { - deinit_action("speech_to_text"); - } + parameters_ = parameters; - virtual std::optional)>> - trigger(std::shared_ptr parameters = nullptr) override - { hri_audio_msgs::action::SpeechToText::Goal goal_msg; - hri_audio_msgs::action::SpeechToText::Result::SharedPtr result_msg; goal_msg.header.stamp = node_->get_clock()->now(); - return [this, &goal_msg, &result_msg](std::shared_ptr result) { - bool action_result = - trigger_action_wait("speech_to_text", goal_msg, result_msg); - result->BoolAttribute("result", action_result); - }; + return goal_msg; + } + + /** + * @brief This generate_result function overrides the generate_result() function from ActionRunner(). Since + * + * @param result message from SpeechToText action + * @return tinyxml2::XMLElement* in the format '' + */ + virtual tinyxml2::XMLElement* + generate_result(const hri_audio_msgs::action::SpeechToText::Result::SharedPtr& result) override + { + std::string text = result->stt_text; + + parameters_->SetAttribute( "text", text.c_str() ); + + return parameters_; } -protected: }; } // namespace capabilities2_runner diff --git a/capabilities2_runner_audio/include/capabilities2_runner_audio/speak_runner.hpp b/capabilities2_runner_audio/include/capabilities2_runner_audio/speak_runner.hpp index e69de29..98824a0 100644 --- a/capabilities2_runner_audio/include/capabilities2_runner_audio/speak_runner.hpp +++ b/capabilities2_runner_audio/include/capabilities2_runner_audio/speak_runner.hpp @@ -0,0 +1,85 @@ +#pragma once + +#include +#include + +#include +#include + +#include + +#include + +namespace capabilities2_runner +{ + +/** + * @brief action runner base class + * + * Class to run text_to_speech action based capability + */ +class SpeakerRunner : public ActionRunner +{ +public: + SpeakerRunner() : 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 pointer to function to execute on starting the runner + * @param on_failure pointer to function to execute on failure of the runner + * @param on_success pointer to function to execute on success of the runner + * @param on_stopped pointer to function to execute on stopping the runner + */ + virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config, + std::function on_started = nullptr, + std::function on_failure = nullptr, + std::function on_success = nullptr, + std::function on_stopped = nullptr) override + { + init_action(node, run_config, "text_to_speech", on_started, on_failure, on_success, on_stopped); + } + +protected: + /** + * @brief This generate goal function overrides the generate_goal() function from ActionRunner() + * + * @param parameters XMLElement that contains parameters in the format '' + * @return ActionT::Goal the generated goal + */ + virtual hri_audio_msgs::action::TextToSpeech::Goal generate_goal(tinyxml2::XMLElement* parameters) override + { + parameters_ = parameters; + + const char **text; + parameters_->QueryStringAttribute("text", text); + std::string tts_text(*text); + + hri_audio_msgs::action::TextToSpeech::Goal goal_msg; + + goal_msg.header.stamp = node_->get_clock()->now(); + goal_msg.tts_text = tts_text; + + return goal_msg; + } + + /** + * @brief This generate_result function overrides the generate_result() function from ActionRunner(). Since this does not + * return a value, return value is nullptr + * + * @param result message from SpeechToText action + * @return nullptr + */ + virtual tinyxml2::XMLElement* + generate_result(const hri_audio_msgs::action::TextToSpeech::Result::SharedPtr& result) override + { + return nullptr; + } + +}; + +} // namespace capabilities2_runner \ No newline at end of file diff --git a/capabilities2_runner_audio/interfaces/speech_to_text.yaml b/capabilities2_runner_audio/interfaces/speech_to_text.yaml new file mode 100644 index 0000000..5da3a8f --- /dev/null +++ b/capabilities2_runner_audio/interfaces/speech_to_text.yaml @@ -0,0 +1,15 @@ +# standardised nav2 interface specification +%YAML 1.1 +--- +name: speech_to_text +spec_version: 1 +spec_type: interface +description: "Speech to text capabilities using Audio stack" +interface: + actions: + "speech_to_text": + type: "hri_audio_msgs::action::SpeechToText" + description: "This system allow the robot to listen to sounds in the environment and then interpret that in the text format. The system can + triggered via '' command. After listening it will emit a output value which + has the format of where '$value' represents what the robot heard + as a text. As an example ''' means that the robot heard 'hello'. \ No newline at end of file diff --git a/capabilities2_runner_audio/interfaces/text_to_speech.yaml b/capabilities2_runner_audio/interfaces/text_to_speech.yaml new file mode 100644 index 0000000..d686e0a --- /dev/null +++ b/capabilities2_runner_audio/interfaces/text_to_speech.yaml @@ -0,0 +1,16 @@ +# standardised nav2 interface specification +%YAML 1.1 +--- +name: text_to_speech +spec_version: 1 +spec_type: interface +description: "Text to Speech capabilities using Audio stack" +interface: + actions: + "text_to_speech": + type: "hri_audio_msgs::action::TextToSpeech" + description: "This system allow the robot to speak a given text. The system can triggered via + '' command. where + '$value' represents what the robot has to speak. if the robot recieves an command + '', the robot + will speak 'how are you'. \ No newline at end of file diff --git a/capabilities2_runner_audio/package.xml b/capabilities2_runner_audio/package.xml index 915904c..c0386f7 100644 --- a/capabilities2_runner_audio/package.xml +++ b/capabilities2_runner_audio/package.xml @@ -26,5 +26,21 @@ ament_cmake + + + interfaces/speech_to_text.yaml + + + interfaces/text_to_speech.yaml + + + + + providers/ListenerRunner.yaml + + + providers/SpeakerRunner.yaml + + diff --git a/capabilities2_runner_audio/plugins.xml b/capabilities2_runner_audio/plugins.xml index d43e9c3..5f1e307 100644 --- a/capabilities2_runner_audio/plugins.xml +++ b/capabilities2_runner_audio/plugins.xml @@ -1,7 +1,12 @@ - + A plugin that provide speech to text capability + + + A plugin that provide text to speech capability + + \ No newline at end of file diff --git a/capabilities2_runner_audio/providers/ListenerRunner.yaml b/capabilities2_runner_audio/providers/ListenerRunner.yaml new file mode 100644 index 0000000..0a9429f --- /dev/null +++ b/capabilities2_runner_audio/providers/ListenerRunner.yaml @@ -0,0 +1,9 @@ +# the empty provider +%YAML 1.1 +--- +name: ListenerRunner +spec_type: provider +spec_version: 1.1 +description: The capability provider for the speech_to_text interface +implements: capabilities2_runner_audio/ListenerRunner +runner: capabilities2_runner::ListenerRunner diff --git a/capabilities2_runner_audio/providers/SpeakerRunner.yaml b/capabilities2_runner_audio/providers/SpeakerRunner.yaml new file mode 100644 index 0000000..4cd315b --- /dev/null +++ b/capabilities2_runner_audio/providers/SpeakerRunner.yaml @@ -0,0 +1,9 @@ +# the empty provider +%YAML 1.1 +--- +name: SpeakerRunner +spec_type: provider +spec_version: 1.1 +description: The capability provider for the text_to_speech interface +implements: capabilities2_runner_nav2/SpeakerRunner +runner: capabilities2_runner::SpeakerRunner diff --git a/capabilities2_runner_audio/src/audio_runners.cpp b/capabilities2_runner_audio/src/audio_runners.cpp index f7b73c5..559be66 100644 --- a/capabilities2_runner_audio/src/audio_runners.cpp +++ b/capabilities2_runner_audio/src/audio_runners.cpp @@ -1,6 +1,7 @@ #include #include #include +#include namespace capabilities2_runner { @@ -8,4 +9,5 @@ namespace capabilities2_runner } // register runner plugins -PLUGINLIB_EXPORT_CLASS(capabilities2_runner::VoiceListenerRunner, capabilities2_runner::RunnerBase) +PLUGINLIB_EXPORT_CLASS(capabilities2_runner::ListenerRunner, capabilities2_runner::RunnerBase) +PLUGINLIB_EXPORT_CLASS(capabilities2_runner::SpeakerRunner, capabilities2_runner::RunnerBase) diff --git a/capabilities2_runner_nav2/CMakeLists.txt b/capabilities2_runner_nav2/CMakeLists.txt index 3057740..27fdcf2 100644 --- a/capabilities2_runner_nav2/CMakeLists.txt +++ b/capabilities2_runner_nav2/CMakeLists.txt @@ -57,6 +57,11 @@ install(DIRECTORY include/ DESTINATION include ) +# install interface files +install(DIRECTORY interfaces + DESTINATION share/${PROJECT_NAME} +) + ament_export_include_directories(include) ament_export_libraries(${PROJECT_NAME}) 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 36b5310..958ed93 100644 --- a/capabilities2_runner_nav2/include/capabilities2_runner_nav2/waypoint_runner.hpp +++ b/capabilities2_runner_nav2/include/capabilities2_runner_nav2/waypoint_runner.hpp @@ -32,29 +32,33 @@ class WayPointRunner : public ActionRunner * * @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_started pointer to function to execute on starting the runner + * @param on_failure pointer to function to execute on failure of the runner + * @param on_success pointer to function to execute on success of the runner + * @param on_stopped pointer to function to execute on stopping 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_failure = nullptr, + std::function on_success = nullptr, std::function on_stopped = nullptr) override { - init_action(node, run_config, "follow_waypoints", on_started, on_terminated, on_stopped); + init_action(node, run_config, "follow_waypoints", on_started, on_failure, on_success, on_stopped); } +protected: /** - * @brief trigger the runner - * - @param parameters XMLElement that contains parameters in the format '' - */ - virtual std::optional)>> - trigger(std::shared_ptr parameters = nullptr) + * @brief This generate goal function overrides the generate_goal() function from ActionRunner() + * @param parameters XMLElement that contains parameters in the format + '' + * @return ActionT::Goal the generated goal + */ + virtual nav2_msgs::action::FollowWaypoints::Goal generate_goal(tinyxml2::XMLElement* parameters) override { - tinyxml2::XMLElement* parametersElement = parameters->FirstChildElement("waypointfollower"); + parameters_ = parameters; - parametersElement->QueryDoubleAttribute("x", &x); - parametersElement->QueryDoubleAttribute("y", &y); + parameters_->QueryDoubleAttribute("x", &x); + parameters_->QueryDoubleAttribute("y", &y); nav2_msgs::action::FollowWaypoints::Goal goal_msg; geometry_msgs::msg::PoseStamped pose_msg; @@ -70,44 +74,16 @@ class WayPointRunner : public ActionRunner goal_msg.poses.push_back(pose_msg); - auto goal_handle_future = action_client_->async_send_goal(goal_msg); - - return [this, goal_handle_future](std::shared_ptr result) { - if (rclcpp::spin_until_future_complete(node_, goal_handle_future) != rclcpp::FutureReturnCode::SUCCESS) - { - RCLCPP_ERROR(node_->get_logger(), "send goal call failed"); - return; - } - - auto result_future = action_client_->async_get_result(goal_handle_future.get()); - if (rclcpp::spin_until_future_complete(node_, result_future) != rclcpp::FutureReturnCode::SUCCESS) - { - RCLCPP_ERROR(node_->get_logger(), "get result call failed"); - return; - } - - auto wrapped_result = result_future.get(); - if (wrapped_result.code == rclcpp_action::ResultCode::SUCCEEDED) - { - RCLCPP_INFO(node_->get_logger(), "Waypoint reached"); - result->BoolAttribute("result", true); - } - else - { - RCLCPP_INFO(node_->get_logger(), "Waypoint not reached"); - } - }; + return goal_msg; } -protected: - // not implemented - virtual nav2_msgs::action::FollowWaypoints::Goal - generate_goal(std::shared_ptr parameters) override - { - return nav2_msgs::action::FollowWaypoints::Goal(); - } - - virtual std::shared_ptr + /** + * @brief This generate result function overrides the generate_result() function from ActionRunner(). Since + * this is not used in this context, this returns nullptr + * @param result message from FollowWaypoints action + * @return nullptr + */ + virtual tinyxml2::XMLElement* generate_result(const nav2_msgs::action::FollowWaypoints::Result::SharedPtr& result) override { return nullptr; diff --git a/capabilities2_runner_nav2/interfaces/nav2.yaml b/capabilities2_runner_nav2/interfaces/follow_waypoints.yaml similarity index 59% rename from capabilities2_runner_nav2/interfaces/nav2.yaml rename to capabilities2_runner_nav2/interfaces/follow_waypoints.yaml index 1acd342..befe9a3 100644 --- a/capabilities2_runner_nav2/interfaces/nav2.yaml +++ b/capabilities2_runner_nav2/interfaces/follow_waypoints.yaml @@ -1,15 +1,15 @@ # standardised nav2 interface specification %YAML 1.1 --- -name: nav2 +name: follow_waypoints spec_version: 1 spec_type: interface -description: "Navigational capabilities using Nav2 stack" +description: "Waypoint follower capabilities using Nav2 stack" interface: actions: "follow_waypoints": type: "nav2_msgs::action::FollowWaypoints" description: "This system allow the robot to navigate to a given two dimensional coordinate - given via '' command. '$value' represents - a value in meters. As an example ' means the + given via '' command. '$value' represents + a value in meters. As an example ' means the robot will move 1.2 meters forward and 0.8 meters to the right side." \ No newline at end of file diff --git a/capabilities2_runner_nav2/package.xml b/capabilities2_runner_nav2/package.xml index 4f32f58..50b6505 100644 --- a/capabilities2_runner_nav2/package.xml +++ b/capabilities2_runner_nav2/package.xml @@ -25,5 +25,15 @@ ament_cmake + + + + interfaces/follow_waypoints.yaml + + + + + providers/WaypointRunner.yaml + diff --git a/capabilities2_runner_nav2/providers/WaypointRunner.yaml b/capabilities2_runner_nav2/providers/WaypointRunner.yaml new file mode 100644 index 0000000..e970dd3 --- /dev/null +++ b/capabilities2_runner_nav2/providers/WaypointRunner.yaml @@ -0,0 +1,9 @@ +# the empty provider +%YAML 1.1 +--- +name: WaypointRunner +spec_type: provider +spec_version: 1.1 +description: The capability provider for the follow_waypoint interface +implements: capabilities2_runner_nav2/WaypointRunner +runner: capabilities2_runner::WayPointRunner diff --git a/capabilities2_server/include/capabilities2_server/capabilities_api.hpp b/capabilities2_server/include/capabilities2_server/capabilities_api.hpp index d725b8c..44d71ae 100644 --- a/capabilities2_server/include/capabilities2_server/capabilities_api.hpp +++ b/capabilities2_server/include/capabilities2_server/capabilities_api.hpp @@ -132,7 +132,7 @@ class CapabilitiesAPI * @param capability * @param parameters */ - void trigger_capability(const std::string& capability, std::shared_ptr parameters = nullptr) + void trigger_capability(const std::string& capability, tinyxml2::XMLElement* parameters = nullptr) { // trigger the runner try diff --git a/capabilities2_server/include/capabilities2_server/runner_cache.hpp b/capabilities2_server/include/capabilities2_server/runner_cache.hpp index e922dea..f02b674 100644 --- a/capabilities2_server/include/capabilities2_server/runner_cache.hpp +++ b/capabilities2_server/include/capabilities2_server/runner_cache.hpp @@ -84,7 +84,7 @@ class RunnerCache * @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) + void trigger_runner(const std::string& capability, tinyxml2::XMLElement* parameters = nullptr) { // is the runner in the cache if (running(capability)) From 2ebac9e82e72ab55eeb442c468813e7535c42464 Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Mon, 7 Oct 2024 13:39:49 +1100 Subject: [PATCH 002/133] added progress so sar --- capabilities2_runner/src/standard_runners.cpp | 9 +- capabilities2_runner_audio/CMakeLists.txt | 4 + capabilities2_runner_nav2/CMakeLists.txt | 5 + .../capabilities2_server/capabilities_api.hpp | 92 ++++++++++++++++--- .../capabilities_server.hpp | 15 ++- .../capabilities2_server/runner_cache.hpp | 52 ++++++----- 6 files changed, 130 insertions(+), 47 deletions(-) diff --git a/capabilities2_runner/src/standard_runners.cpp b/capabilities2_runner/src/standard_runners.cpp index e2c1cf5..1e5616b 100644 --- a/capabilities2_runner/src/standard_runners.cpp +++ b/capabilities2_runner/src/standard_runners.cpp @@ -9,11 +9,12 @@ class DummyRunner : public RunnerBase public: void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config, std::function on_started = nullptr, - std::function on_terminated = nullptr, + std::function on_failure = nullptr, + std::function on_success = nullptr, std::function on_stopped = nullptr) override { // init the base runner - init_base(node, run_config, on_started, on_terminated, on_stopped); + init_base(node, run_config, on_started, on_failure, on_success, on_stopped); // do nothing } @@ -27,8 +28,8 @@ class DummyRunner : public RunnerBase // stop the runner } - std::optional)>> - trigger(std::shared_ptr parameters) override + std::optional> + trigger(tinyxml2::XMLElement* parameters) override { return std::nullopt; } diff --git a/capabilities2_runner_audio/CMakeLists.txt b/capabilities2_runner_audio/CMakeLists.txt index b2a54bd..8a7d9dd 100644 --- a/capabilities2_runner_audio/CMakeLists.txt +++ b/capabilities2_runner_audio/CMakeLists.txt @@ -60,6 +60,10 @@ install(DIRECTORY interfaces DESTINATION share/${PROJECT_NAME} ) +# install providers files +install(DIRECTORY providers + DESTINATION share/${PROJECT_NAME} +) ament_export_include_directories(include) ament_export_libraries(${PROJECT_NAME}) diff --git a/capabilities2_runner_nav2/CMakeLists.txt b/capabilities2_runner_nav2/CMakeLists.txt index 27fdcf2..f15e112 100644 --- a/capabilities2_runner_nav2/CMakeLists.txt +++ b/capabilities2_runner_nav2/CMakeLists.txt @@ -62,6 +62,11 @@ install(DIRECTORY interfaces DESTINATION share/${PROJECT_NAME} ) +# install providers files +install(DIRECTORY providers + DESTINATION share/${PROJECT_NAME} +) + ament_export_include_directories(include) ament_export_libraries(${PROJECT_NAME}) diff --git a/capabilities2_server/include/capabilities2_server/capabilities_api.hpp b/capabilities2_server/include/capabilities2_server/capabilities_api.hpp index e966b0a..3f4437c 100644 --- a/capabilities2_server/include/capabilities2_server/capabilities_api.hpp +++ b/capabilities2_server/include/capabilities2_server/capabilities_api.hpp @@ -66,14 +66,14 @@ class CapabilitiesAPI * @param on_stopped event triggered at the shutdown of the runner by the capabilities server * @param on_terminated event triggered at the failure of the runner by the action or server side. */ - void init_events(std::function on_started, - std::function on_stopped, - std::function on_terminated) - { - runner_cache_.set_on_started(on_started); - runner_cache_.set_on_stopped(on_stopped); - runner_cache_.set_on_terminated(on_terminated); - } + // void init_events(std::function on_started, + // std::function on_stopped, + // std::function on_terminated) + // { + // runner_cache_.set_on_started(on_started); + // runner_cache_.set_on_stopped(on_stopped); + // runner_cache_.set_on_terminated(on_terminated); + // } /** * @brief Start a capability. Internal function only. Do not used this function externally. @@ -81,8 +81,16 @@ class CapabilitiesAPI * @param node ros node pointer of the ros server * @param capability capability name to be started * @param provider provider of the capability + * @param on_started pointer to function to execute on starting the runner + * @param on_failure pointer to function to execute on failure of the runner + * @param on_success pointer to function to execute on success of the runner + * @param on_stopped pointer to function to execute on stopping the runner */ - void start_capability(rclcpp::Node::SharedPtr node, const std::string& capability, const std::string& provider) + void start_capability(rclcpp::Node::SharedPtr node, const std::string& capability, const std::string& provider, + std::function on_started = nullptr, + std::function on_failure = nullptr, + std::function on_success = nullptr, + std::function on_stopped = nullptr) { // get the running model from the db models::running_model_t running = cap_db_->get_running(provider); @@ -110,7 +118,7 @@ class CapabilitiesAPI // TODO: consider the logic for multiple runners per capability try { - runner_cache_.add_runner(node, capability, run_config); + runner_cache_.add_runner(node, capability, run_config, on_started, on_failure, on_success, on_stopped); // log RCLCPP_INFO(node_logging_interface_ptr_->get_logger(), "started capability: %s with provider: %s", @@ -122,12 +130,41 @@ class CapabilitiesAPI } } + /** + * @brief Start the dependencies of a capability. Internal function only. Do not used this function externally. + * + * @param node ros node pointer of the ros server + * @param bond_id bond_id for the capability + * @param capability capability name to be started + * @param provider provider of the capability + */ + void start_dependents(rclcpp::Node::SharedPtr node, const std::string& bond_id, + const std::string& capability, const std::string& provider) + { + // Create bond id + bond_cache_.add_bond(capability, bond_id); + + // get the running model from the db + models::running_model_t running = cap_db_->get_running(provider); + + // start all dependencies + // go through the running model and start the necessary dependencies + for (const auto& run : running.dependencies) + { + RCLCPP_INFO(node_logging_interface_ptr_->get_logger(), "found dependency: %s", run.interface.c_str()); + + // make an internal 'use' bond for the capability dependency + bind_dependency(run.interface); + + // add the runner to the cache + start_capability(node, run.interface, run.provider); + } + } /** * @brief trigger a capability * - * This is a new function for a capability provider (runner) - * that is already started but has additional parameters to be triggered - * This function can be used externally. + * This is a new function for a capability provider (runner) that is already started but has + * additional parameters to be triggered. This function can be used externally. * * @param capability * @param parameters @@ -238,6 +275,35 @@ class CapabilitiesAPI start_capability(node, capability, provider); } + /** + * @brief Connect two capabilities sequentially. This will create a bond ids for the requested instances and, build + * a event callback for sequential triggering and start the capabilities. + * + * @param node ros node pointer of the ros server + * @param capability capability name to be started + * @param provider provider of the capability + */ + void create_serial_connection(rclcpp::Node::SharedPtr node, const std::string& bond_id, + const std::string& source_capability, const std::string& source_provider, + const std::string& target_capability, const std::string& target_provider) + { + + + + try + { + runner_cache_.add_runner(node, capability, cap_db_->get_run_config(provider), on_started, on_failure, on_success, on_stopped); + + // log + RCLCPP_INFO(node_logging_interface_ptr_->get_logger(), "started capability: %s with provider: %s", + capability.c_str(), provider.c_str()); + } + catch (const capabilities2_runner::runner_exception& e) + { + RCLCPP_WARN(node_logging_interface_ptr_->get_logger(), "could not start runner: %s", e.what()); + } + } + // capability api void add_capability(const capabilities2_msgs::msg::CapabilitySpec& spec) { diff --git a/capabilities2_server/include/capabilities2_server/capabilities_server.hpp b/capabilities2_server/include/capabilities2_server/capabilities_server.hpp index 489b154..2e6dd10 100644 --- a/capabilities2_server/include/capabilities2_server/capabilities_server.hpp +++ b/capabilities2_server/include/capabilities2_server/capabilities_server.hpp @@ -418,8 +418,8 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI void handle_accepted( const std::shared_ptr> goal_handle) { - fabric_launch_thread = - std::make_unique(std::bind(&CapabilitiesServer::launch_fabric, this, std::placeholders::_1), goal_handle); + fabric_launch_thread = std::make_unique( + std::bind(&CapabilitiesServer::launch_fabric, this, std::placeholders::_1), goal_handle); } private: @@ -433,19 +433,16 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI { const auto goal = goal_handle->get_goal(); - // execute us_capability for all the connections received + // start all runners and interfaces that the connections depend on for (const auto& connection : goal->connections) { - if (connection.source.capability != "start") - use_capability(shared_from_this(), connection.source.capability, connection.source.provider, goal->bond_id); - - use_capability(shared_from_this(), connection.target.capability, connection.target.provider, goal->bond_id); + start_dependents(shared_from_this(), goal->bond_id, connection.target.capability, connection.target.provider); } // establish relationships for all the connections received - for (const auto& connection : goal->connections) + for (auto connection = goal->connections.rbegin(); connection != goal->connections.rend(); ++connection) { - // if (connection.source.capability != "start") + if (*connection.source.capability != "start") } // trigger starting connections diff --git a/capabilities2_server/include/capabilities2_server/runner_cache.hpp b/capabilities2_server/include/capabilities2_server/runner_cache.hpp index f02b674..cc32427 100644 --- a/capabilities2_server/include/capabilities2_server/runner_cache.hpp +++ b/capabilities2_server/include/capabilities2_server/runner_cache.hpp @@ -29,9 +29,10 @@ class RunnerCache public: RunnerCache() : runner_loader_("capabilities2_runner", "capabilities2_runner::RunnerBase") { - on_started = nullptr; - on_stopped = nullptr; - on_terminated = nullptr; + // on_started = nullptr; + // on_stopped = nullptr; + // on_failure = nullptr; + // on_success = nullptr; } /** @@ -40,9 +41,17 @@ class RunnerCache * @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 on_started pointer to function to execute on starting the runner + * @param on_failure pointer to function to execute on failure of the runner + * @param on_success pointer to function to execute on success of the runner + * @param on_stopped pointer to function to execute on stopping the runner */ void add_runner(rclcpp::Node::SharedPtr node, const std::string& capability, - const models::run_config_model_t& run_config) + const models::run_config_model_t& run_config, + std::function on_started = nullptr, + std::function on_failure = nullptr, + std::function on_success = nullptr, + std::function on_stopped = nullptr) { // if the runner exists then throw an error if (running(capability)) @@ -73,7 +82,7 @@ class RunnerCache } // start the runner - runner_cache_[capability]->start(node, run_config.to_runner_opts(), on_started, on_terminated, on_stopped); + runner_cache_[capability]->start(node, run_config.to_runner_opts(), on_started, on_failure, on_success, on_stopped); } /** @@ -203,30 +212,30 @@ class RunnerCache * * @param cb callback function pointer */ - void set_on_started(std::function cb) - { - on_started = cb; - } + // 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; - } + // void set_on_stopped(std::function cb) + // { + // on_stopped = cb; + // } /** - * @brief Callback function for 'on_terminated' event + * @brief Callback function for 'on_failure' event * * @param cb callback function pointer */ - void set_on_terminated(std::function cb) - { - on_terminated = cb; - } + // void set_on_terminated(std::function cb) + // { + // on_failure = cb; + // } private: // map capability to running model @@ -237,9 +246,10 @@ class RunnerCache pluginlib::ClassLoader runner_loader_; // event callbacks - std::function on_started; - std::function on_stopped; - std::function on_terminated; + // std::function on_started; + // std::function on_stopped; + // std::function on_failure; + // std::function on_success; }; } // namespace capabilities2_server From 8f74fc59b2fea5667952054e6ca5a5e6c7f2d862 Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Thu, 17 Oct 2024 16:29:04 +1100 Subject: [PATCH 003/133] updated executor and runners --- capabilities2_executor/config/.gitkeep | 0 .../capabilities_executor.hpp | 72 ++- .../capabilities_file_parser.hpp | 201 ++++--- .../capabilities_xml_parser.hpp | 521 +++++++++--------- .../structs/connection.hpp | 32 +- capabilities2_executor/launch/.gitkeep | 0 .../msg/CapabilityConnection.msg | 24 +- .../capabilities2_runner/action_runner.hpp | 134 ++--- .../capabilities2_runner/encap_runner.hpp | 16 +- .../capabilities2_runner/launch_runner.hpp | 14 +- .../capabilities2_runner/runner_base.hpp | 170 ++++-- capabilities2_runner/src/standard_runners.cpp | 8 +- .../listen_runner.hpp | 12 +- .../speak_runner.hpp | 12 +- .../waypoint_runner.hpp | 12 +- 15 files changed, 665 insertions(+), 563 deletions(-) create mode 100644 capabilities2_executor/config/.gitkeep create mode 100644 capabilities2_executor/launch/.gitkeep diff --git a/capabilities2_executor/config/.gitkeep b/capabilities2_executor/config/.gitkeep new file mode 100644 index 0000000..e69de29 diff --git a/capabilities2_executor/include/capabilities2_executor/capabilities_executor.hpp b/capabilities2_executor/include/capabilities2_executor/capabilities_executor.hpp index 89dd936..d956b61 100644 --- a/capabilities2_executor/include/capabilities2_executor/capabilities_executor.hpp +++ b/capabilities2_executor/include/capabilities2_executor/capabilities_executor.hpp @@ -34,9 +34,7 @@ class CapabilitiesExecutor : public rclcpp::Node public: CapabilitiesExecutor(const rclcpp::NodeOptions& options = rclcpp::NodeOptions()) : Node("Capabilities2_Executor", options) { - control_tag_list.push_back("sequential"); - control_tag_list.push_back("parallel"); - control_tag_list.push_back("recovery"); + control_tag_list = capabilities2_xml_parser::get_control_list(); this->planner_server_ = rclcpp_action::create_server( this, "~/capabilities", std::bind(&CapabilitiesExecutor::handle_goal, this, std::placeholders::_1, std::placeholders::_2), @@ -320,7 +318,7 @@ class CapabilitiesExecutor : public rclcpp::Node tinyxml2::XMLElement* plan = capabilities2_xml_parser::get_plan(document); // Extract the connections from the plan - capabilities2_xml_parser::extract_connections(plan, connection_list); + capabilities2_xml_parser::extract_connections(plan, connection_map); // estasblish the bond with the server if (!establish_bond()) @@ -340,23 +338,59 @@ class CapabilitiesExecutor : public rclcpp::Node capabilities2_msgs::msg::CapabilityConnection connection_msg; - for (const auto& connection : connection_list) + for (const auto& [key, value] : connection_map) { - connection_msg.source.capability = connection.source_event; - connection_msg.source.provider = connection.source_provider; - connection_msg.target.capability = connection.target_event; - connection_msg.target.provider = connection.target_provider; + RCLCPP_INFO(this->get_logger(), "Node : %i", key); - if (connection.connection == capabilities2_executor::connection_type_t::ON_SUCCESS_START) - connection_msg.connection_type = capabilities2_msgs::msg::CapabilityConnection::ON_SUCCESS_START; + if (capabilities2_xml_parser::convert_to_string(value.source.parameters, connection_msg.source_parameters)) + { + connection_msg.source.capability = value.source.runner; + connection_msg.source.provider = value.source.provider; + + RCLCPP_INFO(this->get_logger(), "Source Capability : %s", connection_msg.source.capability.c_str()); + RCLCPP_INFO(this->get_logger(), "Source Provider : %s", connection_msg.source.provider.c_str()); + RCLCPP_INFO(this->get_logger(), "Source Parameters : %s", connection_msg.source_parameters.c_str()); + } + + if (capabilities2_xml_parser::convert_to_string(value.target_on_start.parameters, connection_msg.target_on_start_parameters)) + { + connection_msg.target_on_start.capability = value.target_on_start.runner; + connection_msg.target_on_start.provider = value.target_on_start.provider; + + RCLCPP_INFO(this->get_logger(), "Triggered on start Capability : %s", connection_msg.target_on_start.capability.c_str()); + RCLCPP_INFO(this->get_logger(), "Triggered on start Provider : %s", connection_msg.target_on_start.provider.c_str()); + RCLCPP_INFO(this->get_logger(), "Triggered on start Parameters : %s", connection_msg.target_on_start_parameters.c_str()); + } + + if (capabilities2_xml_parser::convert_to_string(value.target_on_stop.parameters, connection_msg.target_on_stop_parameters)) + { + connection_msg.target_on_stop.capability = value.target_on_stop.runner; + connection_msg.target_on_stop.provider = value.target_on_stop.provider; + + RCLCPP_INFO(this->get_logger(), "Triggered on stop Capability : %s", connection_msg.target_on_stop.capability.c_str()); + RCLCPP_INFO(this->get_logger(), "Triggered on stop Provider : %s", connection_msg.target_on_stop.provider.c_str()); + RCLCPP_INFO(this->get_logger(), "Triggered on stop Parameters : %s", connection_msg.target_on_stop_parameters.c_str()); + } + + if (capabilities2_xml_parser::convert_to_string(value.target_on_success.parameters, connection_msg.target_on_success_parameters)) + { + connection_msg.target_on_success.capability = value.target_on_success.runner; + connection_msg.target_on_success.provider = value.target_on_success.provider; - if (connection.connection == capabilities2_executor::connection_type_t::ON_START_START) - connection_msg.connection_type = capabilities2_msgs::msg::CapabilityConnection::ON_START_START; + RCLCPP_INFO(this->get_logger(), "Triggered on success Capability : %s", connection_msg.target_on_success.capability.c_str()); + RCLCPP_INFO(this->get_logger(), "Triggered on success Provider : %s", connection_msg.target_on_success.provider.c_str()); + RCLCPP_INFO(this->get_logger(), "Triggered on success Parameters : %s", connection_msg.target_on_success_parameters.c_str()); + } - if (connection.connection == capabilities2_executor::connection_type_t::ON_FAILURE_START) - connection_msg.connection_type = capabilities2_msgs::msg::CapabilityConnection::ON_FAILURE_START; + if (capabilities2_xml_parser::convert_to_string(value.target_on_failure.parameters, connection_msg.target_on_failure_parameters)) + { + connection_msg.target_on_failure.capability = value.target_on_failure.runner; + connection_msg.target_on_failure.provider = value.target_on_failure.provider; - connection_msg.target_parameters = capabilities2_xml_parser::convert_to_string(connection.event_element); + RCLCPP_INFO(this->get_logger(), "Triggered on failure Capability : %s", connection_msg.target_on_failure.capability.c_str()); + RCLCPP_INFO(this->get_logger(), "Triggered on failure Provider : %s", connection_msg.target_on_failure.provider.c_str()); + RCLCPP_INFO(this->get_logger(), "Triggered on failure Parameters : %s", connection_msg.target_on_failure_parameters.c_str()); + } connection_goal_msg.connections.push_back(connection_msg); } @@ -432,9 +466,9 @@ class CapabilitiesExecutor : public rclcpp::Node for (const auto& failed_connection : result.result->failed_connections) { - RCLCPP_ERROR(this->get_logger(), "Failed Events : %s", failed_connection.target_parameters.c_str()); + RCLCPP_ERROR(this->get_logger(), "Failed Events : %s", failed_connection.source_parameters.c_str()); - result_out->failed_elements.push_back(failed_connection.target_parameters); + result_out->failed_elements.push_back(failed_connection.source_parameters); } server_goal_handle->canceled(result_out); @@ -470,7 +504,7 @@ class CapabilitiesExecutor : public rclcpp::Node tinyxml2::XMLDocument document; /** vector of connections */ - std::vector connection_list; + std::map connection_map; /** Execution Thread */ std::shared_ptr execution_thread; diff --git a/capabilities2_executor/include/capabilities2_executor/capabilities_file_parser.hpp b/capabilities2_executor/include/capabilities2_executor/capabilities_file_parser.hpp index 5164897..c0263d9 100644 --- a/capabilities2_executor/include/capabilities2_executor/capabilities_file_parser.hpp +++ b/capabilities2_executor/include/capabilities2_executor/capabilities_file_parser.hpp @@ -23,111 +23,108 @@ class CapabilitiesFileParser : public rclcpp::Node { public: - CapabilitiesFileParser(const rclcpp::NodeOptions &options = rclcpp::NodeOptions()) - : Node("Capabilities2_File_Parser", options) - { - declare_parameter("plan_file_path", "plan.xml"); - plan_file_path = get_parameter("plan_file_path").as_string(); - - this->client_ptr_ = rclcpp_action::create_client(this, "~/capabilities"); - - this->timer_ = this->create_wall_timer(std::chrono::milliseconds(500), std::bind(&CapabilitiesFileParser::send_goal, this)); - } - - void send_goal() - { - this->timer_->cancel(); - - // try to load the file - tinyxml2::XMLError xml_status = document.LoadFile(plan_file_path.c_str()); - - // check if the file loading failed - if (xml_status != tinyxml2::XMLError::XML_SUCCESS) - { - RCLCPP_INFO(this->get_logger(), "Loading the file from path : %s failed", plan_file_path.c_str()); - rclcpp::shutdown(); - } - - if (!this->client_ptr_->wait_for_action_server()) - { - RCLCPP_ERROR(this->get_logger(), "Action server not available after waiting"); - rclcpp::shutdown(); - } - - auto goal_msg = capabilities2_msgs::action::Plan::Goal(); - goal_msg.plan = capabilities2_xml_parser::convert_to_string(document); - - RCLCPP_INFO(this->get_logger(), "Sending goal"); - - auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); - - // send goal options - // goal response callback - send_goal_options.goal_response_callback = - [this](const rclcpp_action::ClientGoalHandle::SharedPtr &goal_handle) - { - if (!goal_handle) - { - RCLCPP_ERROR(this->get_logger(), "Goal was rejected by server"); - } - else - { - RCLCPP_INFO(this->get_logger(), "Goal accepted by server, waiting for result"); - } - }; - - // result callback - send_goal_options.result_callback = - [this](const rclcpp_action::ClientGoalHandle::WrappedResult &result) - { - switch (result.code) - { - case rclcpp_action::ResultCode::SUCCEEDED: - break; - case rclcpp_action::ResultCode::ABORTED: - RCLCPP_ERROR(this->get_logger(), "Goal was aborted"); - return; - case rclcpp_action::ResultCode::CANCELED: - RCLCPP_ERROR(this->get_logger(), "Goal was canceled"); - return; - default: - RCLCPP_ERROR(this->get_logger(), "Unknown result code"); - return; - } - - if (result.result->success) - { - RCLCPP_ERROR(this->get_logger(), "Plan executed successfully"); - } - else - { - RCLCPP_ERROR(this->get_logger(), "Plan failed to complete"); - - if (result.result->failed_elements.size() > 0) - { - RCLCPP_ERROR(this->get_logger(), "Plan failed due to incompatible XMLElements in the plan"); - - for (const auto &failed_element : result.result->failed_elements) - RCLCPP_ERROR(this->get_logger(), "Failed Elements : %s", failed_element.c_str()); - } - } - - rclcpp::shutdown(); - }; - - this->client_ptr_->async_send_goal(goal_msg, send_goal_options); - } + CapabilitiesFileParser(const rclcpp::NodeOptions& options = rclcpp::NodeOptions()) : Node("Capabilities2_File_Parser", options) + { + declare_parameter("plan_file_path", "plan.xml"); + plan_file_path = get_parameter("plan_file_path").as_string(); + + this->client_ptr_ = rclcpp_action::create_client(this, "~/capabilities"); + + this->timer_ = this->create_wall_timer(std::chrono::milliseconds(500), std::bind(&CapabilitiesFileParser::send_goal, this)); + } + + void send_goal() + { + this->timer_->cancel(); + + // try to load the file + tinyxml2::XMLError xml_status = document.LoadFile(plan_file_path.c_str()); + + // check if the file loading failed + if (xml_status != tinyxml2::XMLError::XML_SUCCESS) + { + RCLCPP_INFO(this->get_logger(), "Loading the file from path : %s failed", plan_file_path.c_str()); + rclcpp::shutdown(); + } + + if (!this->client_ptr_->wait_for_action_server()) + { + RCLCPP_ERROR(this->get_logger(), "Action server not available after waiting"); + rclcpp::shutdown(); + } + + auto goal_msg = capabilities2_msgs::action::Plan::Goal(); + goal_msg.plan = capabilities2_xml_parser::convert_to_string(document); + + RCLCPP_INFO(this->get_logger(), "Sending goal"); + + auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); + + // send goal options + // goal response callback + send_goal_options.goal_response_callback = + [this](const rclcpp_action::ClientGoalHandle::SharedPtr& goal_handle) { + if (!goal_handle) + { + RCLCPP_ERROR(this->get_logger(), "Goal was rejected by server"); + } + else + { + RCLCPP_INFO(this->get_logger(), "Goal accepted by server, waiting for result"); + } + }; + + // result callback + send_goal_options.result_callback = + [this](const rclcpp_action::ClientGoalHandle::WrappedResult& result) { + switch (result.code) + { + case rclcpp_action::ResultCode::SUCCEEDED: + break; + case rclcpp_action::ResultCode::ABORTED: + RCLCPP_ERROR(this->get_logger(), "Goal was aborted"); + return; + case rclcpp_action::ResultCode::CANCELED: + RCLCPP_ERROR(this->get_logger(), "Goal was canceled"); + return; + default: + RCLCPP_ERROR(this->get_logger(), "Unknown result code"); + return; + } + + if (result.result->success) + { + RCLCPP_ERROR(this->get_logger(), "Plan executed successfully"); + } + else + { + RCLCPP_ERROR(this->get_logger(), "Plan failed to complete"); + + if (result.result->failed_elements.size() > 0) + { + RCLCPP_ERROR(this->get_logger(), "Plan failed due to incompatible XMLElements in the plan"); + + for (const auto& failed_element : result.result->failed_elements) + RCLCPP_ERROR(this->get_logger(), "Failed Elements : %s", failed_element.c_str()); + } + } + + rclcpp::shutdown(); + }; + + this->client_ptr_->async_send_goal(goal_msg, send_goal_options); + } private: - /** File Path link */ - std::string plan_file_path; + /** File Path link */ + std::string plan_file_path; - /** XML Document */ - tinyxml2::XMLDocument document; + /** XML Document */ + tinyxml2::XMLDocument document; - /** action client */ - rclcpp_action::Client::SharedPtr client_ptr_; + /** action client */ + rclcpp_action::Client::SharedPtr client_ptr_; - /** action server */ - rclcpp::TimerBase::SharedPtr timer_; + /** action server */ + rclcpp::TimerBase::SharedPtr timer_; }; \ No newline at end of file diff --git a/capabilities2_executor/include/capabilities2_executor/capabilities_xml_parser.hpp b/capabilities2_executor/include/capabilities2_executor/capabilities_xml_parser.hpp index 102d345..65ff5f6 100644 --- a/capabilities2_executor/include/capabilities2_executor/capabilities_xml_parser.hpp +++ b/capabilities2_executor/include/capabilities2_executor/capabilities_xml_parser.hpp @@ -5,261 +5,266 @@ namespace capabilities2_xml_parser { - /** - * @brief extract elements related plan - * - * @param document XML document to extract plan from - * - * @return plan in the form of tinyxml2::XMLElement* - */ - tinyxml2::XMLElement *get_plan(tinyxml2::XMLDocument &document) - { - return document.FirstChildElement("Plan"); - } - - /** - * @brief search a string in a vector of strings - * - * @param list vector of strings to be searched - * @param value string to be searched in the vector - * - * @return `true` if value is found in list and `false` otherwise - */ - bool search(std::vector list, std::string value) - { - return (std::find(list.begin(), list.end(), value) != list.end()); - } - - /** - * @brief check if the element in question is the last in its level - * - * @param element element to be checked - * - * @return `true` if last and `false` otherwise - */ - bool isLastElement(tinyxml2::XMLElement *element) - { - return (element == element->Parent()->LastChildElement()); - } - - /** - * @brief check if the xml document has valid plan tags - * - * @param document XML document in question - * - * @return `true` if its valid and `false` otherwise - */ - bool check_plan_tag(tinyxml2::XMLDocument &document) - { - std::string plan_tag(document.FirstChildElement()->Name()); - - if (plan_tag == "Plan") - return true; - else - return false; - } - - /** - * @brief convert XMLElement to std::string - * - * @param element element to be converted - * - * @return std::string converted element - */ - std::string convert_to_string(tinyxml2::XMLElement *element) - { - tinyxml2::XMLPrinter printer; - - element->Accept(&printer); - - std::string value = printer.CStr(); - - return value; - } - - /** - * @brief convert XMLDocument to std::string - * - * @param document element to be converted - * - * @return std::string converted document - */ - std::string convert_to_string(tinyxml2::XMLDocument &document) - { - tinyxml2::XMLPrinter printer; - - document.Accept(&printer); - - std::string value = printer.CStr(); - - return value; - } - - /** - * @brief check the plan for invalid/unsupported control and event tags - * uses recursive approach to go through the plan - * - * @param element XML Element to be evaluated - * @param events_list list containing valid event tags - * @param providers_list list containing providers - * @param control_list list containing valid control tags - * - * @return `true` if element valid and supported and `false` otherwise - */ - bool check_tags(tinyxml2::XMLElement *element, - std::vector &events_list, - std::vector &providers_list, - std::vector &control_list) - { - const char **name; - const char **provider; - - element->QueryStringAttribute("name", name); - element->QueryStringAttribute("provider", provider); - - std::string typetag(element->Name()); - std::string nametag(*name); - std::string providertag(*provider); - - bool hasChildren = !element->NoChildren(); - bool hasSiblings = !capabilities2_xml_parser::isLastElement(element); - bool foundInControl = capabilities2_xml_parser::search(control_list, nametag); - bool foundInEvents = capabilities2_xml_parser::search(events_list, nametag); - bool foundInProviders = capabilities2_xml_parser::search(providers_list, providertag); - bool returnValue = true; - - if (typetag == "Control") - { - if (foundInControl and hasChildren) - returnValue = returnValue and capabilities2_xml_parser::check_tags(element->FirstChildElement(), events_list, providers_list, control_list); - - if (foundInControl and hasSiblings) - returnValue = returnValue and capabilities2_xml_parser::check_tags(element->NextSiblingElement(), events_list, providers_list, control_list); - - if (foundInControl and !hasSiblings) - returnValue = returnValue; - - if (!foundInControl) - return false; - } - else if (typetag == "Event") - { - if (foundInEvents and foundInProviders and hasSiblings) - returnValue = returnValue and capabilities2_xml_parser::check_tags(element->NextSiblingElement(), events_list, providers_list, control_list); - - if (foundInEvents and foundInProviders and !hasSiblings) - returnValue = returnValue; - - if (!foundInEvents) - return false; - - if (!foundInProviders) - return false; - } - else - { - return false; - } - - return returnValue; - } - - /** - * @brief Adds an event connection to the capabilities2 fabric - * - * @param source_event the source event from which the connection would start - * @param source_provider provider of the source event - * @param target_event the target event which will be connected via the connection - * @param target_provider provider of the target event - * @param connection the type of connection - * @param event_element the tinyxml2::XMLElement which contains runtime parameters - * - * @return The connection object - */ - capabilities2_executor::connection_t create_connection(std::string source_event, std::string source_provider, - std::string target_event, std::string target_provider, - capabilities2_executor::connection_type_t connection, tinyxml2::XMLElement *event_element) - { - capabilities2_executor::connection_t connect; - - connect.source_event = source_event; - connect.source_provider = source_provider; - connect.target_event = target_event; - connect.target_provider = target_provider; - connect.connection = connection; - connect.event_element = event_element; - - return connect; - } - - /** - * @brief parse through the plan and extract the connections - * - * @param element XML Element to be evaluated - * @param connection_list std::vector containing extracted connections - * @param connection the type of connection - * @param source_event source event name. if not provided, "start" is taken as default - * @param source_provider provider of the source event, if not provided "" is taken as default - * @param target_event target event name. if not provided, "stop" is taken as default - * @param target_provider provider of the target event. if not provided, "stop" is taken as default - */ - void extract_connections(tinyxml2::XMLElement *element, std::vector &connection_list, - capabilities2_executor::connection_type_t connection = capabilities2_executor::connection_type_t::ON_SUCCESS_START, - std::string source_event = "start", std::string source_provider = "", - std::string target_event = "stop", std::string target_provider = "") - { - const char **name; - const char **provider; - - element->QueryStringAttribute("name", name); - element->QueryStringAttribute("provider", provider); - - std::string typetag(element->Name()); - std::string nametag(*name); - std::string providertag(*provider); - - bool hasChildren = !element->NoChildren(); - bool hasSiblings = !capabilities2_xml_parser::isLastElement(element); - - if ((typetag == "Control") and (nametag == "sequential")) - { - if (hasChildren) - capabilities2_xml_parser::extract_connections(element->FirstChildElement(), connection_list, capabilities2_executor::connection_type_t::ON_SUCCESS_START, - source_event, source_provider, target_event, target_provider); - - if (hasSiblings) - capabilities2_xml_parser::extract_connections(element->NextSiblingElement(), connection_list, connection, - source_event, source_provider, target_event, target_provider); - } - else if ((typetag == "Control") and (nametag == "parallel")) - { - if (hasChildren) - capabilities2_xml_parser::extract_connections(element->FirstChildElement(), connection_list, capabilities2_executor::connection_type_t::ON_START_START, - source_event, source_provider, target_event, target_provider); - - if (hasSiblings) - capabilities2_xml_parser::extract_connections(element->NextSiblingElement(), connection_list, connection, - source_event, source_provider, target_event, target_provider); - } - else if ((typetag == "Control") and (nametag == "recovery")) - { - if (hasChildren) - capabilities2_xml_parser::extract_connections(element->FirstChildElement(), connection_list, capabilities2_executor::connection_type_t::ON_FAILURE_START, - source_event, source_provider, target_event, target_provider); - - if (hasSiblings) - capabilities2_xml_parser::extract_connections(element->NextSiblingElement(), connection_list, connection, - source_event, source_provider, target_event, target_provider); - } - else if (typetag == "Event") - { - capabilities2_executor::connection_t connection_obj = create_connection(source_event, source_provider, nametag, providertag, connection, element); - connection_list.push_back(connection_obj); - - if (hasSiblings) - capabilities2_xml_parser::extract_connections(element->NextSiblingElement(), connection_list, connection, - nametag, providertag, target_event, target_provider); - } - } - -} // namespace capabilities2_xml_parser +/** + * @brief extract elements related plan + * + * @param document XML document to extract plan from + * + * @return plan in the form of tinyxml2::XMLElement* + */ +tinyxml2::XMLElement* get_plan(tinyxml2::XMLDocument& document) +{ + return document.FirstChildElement("Plan"); +} + +/** + * @brief search a string in a vector of strings + * + * @param list vector of strings to be searched + * @param value string to be searched in the vector + * + * @return `true` if value is found in list and `false` otherwise + */ +bool search(std::vector list, std::string value) +{ + return (std::find(list.begin(), list.end(), value) != list.end()); +} + +/** + * @brief check if the element in question is the last in its level + * + * @param element element to be checked + * + * @return `true` if last and `false` otherwise + */ +bool isLastElement(tinyxml2::XMLElement* element) +{ + return (element == element->Parent()->LastChildElement()); +} + +/** + * @brief check if the xml document has valid plan tags + * + * @param document XML document in question + * + * @return `true` if its valid and `false` otherwise + */ +bool check_plan_tag(tinyxml2::XMLDocument& document) +{ + std::string plan_tag(document.FirstChildElement()->Name()); + + if (plan_tag == "Plan") + return true; + else + return false; +} + +/** + * @brief convert XMLElement to std::string + * + * @param element XMLElement element to be converted + * @param paramters parameter to hold std::string + * + * @return `true` if element is not nullptr and conversion successful, `false` if element is nullptr + */ +bool convert_to_string(tinyxml2::XMLElement* element, std::string& paramters) +{ + if (element) + { + tinyxml2::XMLPrinter printer; + + element->Accept(&printer); + + paramters = printer.CStr(); + + return true; + } + else + { + false; + } +} + +/** + * @brief convert XMLDocument to std::string + * + * @param document element to be converted + * + * @return std::string converted document + */ +std::string convert_to_string(tinyxml2::XMLDocument& document) +{ + tinyxml2::XMLPrinter printer; + + document.Accept(&printer); + + std::string value = printer.CStr(); + + return value; +} + +/** + * @brief check the plan for invalid/unsupported control and event tags + * uses recursive approach to go through the plan + * + * @param element XML Element to be evaluated + * @param events_list list containing valid event tags + * @param providers_list list containing providers + * @param control_list list containing valid control tags + * + * @return `true` if element valid and supported and `false` otherwise + */ +bool check_tags(tinyxml2::XMLElement* element, std::vector& events_list, std::vector& providers_list, + std::vector& control_list) +{ + const char** name; + const char** provider; + + element->QueryStringAttribute("name", name); + element->QueryStringAttribute("provider", provider); + + std::string typetag(element->Name()); + std::string nametag(*name); + std::string providertag(*provider); + + bool hasChildren = !element->NoChildren(); + bool hasSiblings = !capabilities2_xml_parser::isLastElement(element); + bool foundInControl = capabilities2_xml_parser::search(control_list, nametag); + bool foundInEvents = capabilities2_xml_parser::search(events_list, nametag); + bool foundInProviders = capabilities2_xml_parser::search(providers_list, providertag); + bool returnValue = true; + + if (typetag == "Control") + { + if (foundInControl and hasChildren) + returnValue = returnValue and capabilities2_xml_parser::check_tags(element->FirstChildElement(), events_list, providers_list, control_list); + + if (foundInControl and hasSiblings) + returnValue = returnValue and capabilities2_xml_parser::check_tags(element->NextSiblingElement(), events_list, providers_list, control_list); + + if (foundInControl and !hasSiblings) + returnValue = returnValue; + + if (!foundInControl) + return false; + } + else if (typetag == "Event") + { + if (foundInEvents and foundInProviders and hasSiblings) + returnValue = returnValue and capabilities2_xml_parser::check_tags(element->NextSiblingElement(), events_list, providers_list, control_list); + + if (foundInEvents and foundInProviders and !hasSiblings) + returnValue = returnValue; + + if (!foundInEvents) + return false; + + if (!foundInProviders) + return false; + } + else + { + return false; + } + + return returnValue; +} + +/** + * @brief Returns control xml tags supported in extract_connections method + * + */ +std::vector get_control_list() +{ + std::vector tag_list; + + tag_list.push_back("sequential"); + tag_list.push_back("parallel"); + tag_list.push_back("recovery"); + + return tag_list; +} + +/** + * @brief parse through the plan and extract the connections + * + * @param element XML Element to be evaluated + * @param connections std::map containing extracted connections + * @param connection_id numerical id of the connection + * @param connection_type the type of connection + */ +void extract_connections(tinyxml2::XMLElement* element, std::map& connections, int connection_id = 0, + capabilities2_executor::connection_type_t connection_type = capabilities2_executor::connection_type_t::ON_SUCCESS) +{ + int predecessor_id; + + const char** name; + const char** provider; + + element->QueryStringAttribute("name", name); + element->QueryStringAttribute("provider", provider); + + std::string typetag(element->Name()); + std::string nametag(*name); + std::string providertag(*provider); + + bool hasChildren = !element->NoChildren(); + bool hasSiblings = !capabilities2_xml_parser::isLastElement(element); + + if ((typetag == "Control") and (nametag == "sequential")) + { + if (hasChildren) + capabilities2_xml_parser::extract_connections(element->FirstChildElement(), connections, connection_id, + capabilities2_executor::connection_type_t::ON_SUCCESS); + + if (hasSiblings) + capabilities2_xml_parser::extract_connections(element->NextSiblingElement(), connections, connection_id, connection_type); + } + else if ((typetag == "Control") and (nametag == "parallel")) + { + if (hasChildren) + capabilities2_xml_parser::extract_connections(element->FirstChildElement(), connections, connection_id, + capabilities2_executor::connection_type_t::ON_START); + + if (hasSiblings) + capabilities2_xml_parser::extract_connections(element->NextSiblingElement(), connections, connection_id, connection_type); + } + else if ((typetag == "Control") and (nametag == "recovery")) + { + if (hasChildren) + capabilities2_xml_parser::extract_connections(element->FirstChildElement(), connections, connection_id, + capabilities2_executor::connection_type_t::ON_FAILURE); + + if (hasSiblings) + capabilities2_xml_parser::extract_connections(element->NextSiblingElement(), connections, connection_id, connection_type); + } + else if (typetag == "Event") + { + capabilities2_executor::node_t node; + + node.source.runner = nametag; + node.source.provider = providertag; + node.source.parameters = element; + + predecessor_id = connection_id - 1; + + while (connections.count(connection_id) > 0) + connection_id += 1; + + connections[connection_id] = node; + + if ((connection_type == capabilities2_executor::connection_type_t::ON_SUCCESS) and (connection_id != 0)) + connections[predecessor_id].target_on_success = connections[connection_id].source; + + else if ((connection_type == capabilities2_executor::connection_type_t::ON_START) and (connection_id != 0)) + connections[predecessor_id].target_on_start = connections[connection_id].source; + + else if ((connection_type == capabilities2_executor::connection_type_t::ON_FAILURE) and (connection_id != 0)) + connections[predecessor_id].target_on_failure = connections[connection_id].source; + + if (hasSiblings) + capabilities2_xml_parser::extract_connections(element->NextSiblingElement(), connections, connection_id + 1, connection_type); + } +} + +} // namespace capabilities2_xml_parser diff --git a/capabilities2_executor/include/capabilities2_executor/structs/connection.hpp b/capabilities2_executor/include/capabilities2_executor/structs/connection.hpp index 30b8a21..d6ecc65 100644 --- a/capabilities2_executor/include/capabilities2_executor/structs/connection.hpp +++ b/capabilities2_executor/include/capabilities2_executor/structs/connection.hpp @@ -5,23 +5,25 @@ namespace capabilities2_executor { enum connection_type_t { - ON_START_START, - ON_START_STOP, - ON_SUCCESS_START, - ON_SUCCESS_STOP, - ON_FAILURE_START, - ON_FAILURE_STOP, - ON_TERMINATE_START, - ON_TERMINATE_STOP + ON_START, + ON_SUCCESS, + ON_FAILURE, + ON_STOP }; - struct connection_t { - std::string source_event; - std::string source_provider; - std::string target_event; - std::string target_provider; - connection_type_t connection; - tinyxml2::XMLElement* event_element; + struct connection_t + { + std::string runner; + std::string provider; + tinyxml2::XMLElement* parameters = nullptr; + }; + + struct node_t { + connection_t source; + connection_t target_on_start; + connection_t target_on_stop; + connection_t target_on_success; + connection_t target_on_failure; }; } // namespace capabilities2_executor diff --git a/capabilities2_executor/launch/.gitkeep b/capabilities2_executor/launch/.gitkeep new file mode 100644 index 0000000..e69de29 diff --git a/capabilities2_msgs/msg/CapabilityConnection.msg b/capabilities2_msgs/msg/CapabilityConnection.msg index 04b538b..2b8861e 100644 --- a/capabilities2_msgs/msg/CapabilityConnection.msg +++ b/capabilities2_msgs/msg/CapabilityConnection.msg @@ -1,14 +1,10 @@ -# Declare types of connections -uint8 ON_START_START = 0 -uint8 ON_START_STOP = 1 -uint8 ON_SUCCESS_START = 2 -uint8 ON_SUCCESS_STOP = 3 -uint8 ON_FAILURE_START = 4 -uint8 ON_FAILURE_STOP = 5 -uint8 ON_TERMINATE_START = 6 -uint8 ON_TERMINATE_STOP = 7 - -Capability source -Capability target -uint8 connection_type -string target_parameters \ No newline at end of file +capabilities2_msgs/Capability source +string source_parameters +capabilities2_msgs/Capability target_on_start +string target_on_start_parameters +capabilities2_msgs/Capability target_on_stop +string target_on_stop_parameters +capabilities2_msgs/Capability target_on_success +string target_on_success_parameters +capabilities2_msgs/Capability target_on_failure +string target_on_failure_parameters \ No newline at end of file diff --git a/capabilities2_runner/include/capabilities2_runner/action_runner.hpp b/capabilities2_runner/include/capabilities2_runner/action_runner.hpp index 94e440a..47340bc 100644 --- a/capabilities2_runner/include/capabilities2_runner/action_runner.hpp +++ b/capabilities2_runner/include/capabilities2_runner/action_runner.hpp @@ -36,19 +36,11 @@ class ActionRunner : public RunnerBase * @param node shared pointer to the capabilities node. Allows to use ros node related functionalities * @param run_config runner configuration loaded from the yaml file * @param action_name action name used in the yaml file, used to load specific configuration from the run_config - * @param on_started pointer to function to execute on starting the runner - * @param on_failure pointer to function to execute on failure of the runner - * @param on_success pointer to function to execute on success of the runner - * @param on_stopped pointer to function to execute on stopping the runner */ - 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_failure = nullptr, - std::function on_success = nullptr, - std::function on_stopped = nullptr) + virtual void init_action(rclcpp::Node::SharedPtr node, const runner_opts& run_config, const std::string& action_name) { // initialize the runner base by storing node pointer and run config - init_base(node, run_config, on_started, on_failure, on_success, on_stopped); + init_base(node, run_config); // create an action client action_client_ = rclcpp_action::create_client(node_, action_name); @@ -62,36 +54,6 @@ class ActionRunner : public RunnerBase 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) - { - // send terminated event - if (on_success_) - on_success_(run_config_.interface); - } - else - { - // send terminated event - if (on_failure_) - on_failure_(run_config_.interface); - } - }; } /** @@ -125,9 +87,11 @@ class ActionRunner : public RunnerBase } // publish event - if (on_stopped_) + if (event_tracker[execute_tracker_id].on_stopped) { - on_stopped_(run_config_.interface); + event_tracker[execute_tracker_id].on_stopped( + update_on_stopped(event_tracker[execute_tracker_id].on_stopped_param)); + execute_tracker_id += 1; } }); @@ -154,8 +118,8 @@ class ActionRunner : public RunnerBase trigger(tinyxml2::XMLElement* parameters = nullptr) override { // the action is being triggered out of order - if (!goal_handle_) - throw runner_exception("cannot trigger action that was not started"); + // if (!goal_handle_) + // throw runner_exception("cannot trigger action that was not started"); // if parameters are not provided then cannot proceed if (!parameters) @@ -164,6 +128,48 @@ class ActionRunner : public RunnerBase // generate a goal from parameters if provided typename ActionT::Goal goal_msg = generate_goal(parameters); + // 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 (event_tracker[execute_tracker_id].on_started) + { + event_tracker[execute_tracker_id].on_started( + update_on_started(event_tracker[execute_tracker_id].on_started_param)); + execute_tracker_id += 1; + } + + // 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) + { + // send success event + if (event_tracker[execute_tracker_id].on_success) + { + event_tracker[execute_tracker_id].on_success( + update_on_success(event_tracker[execute_tracker_id].on_success_param)); + execute_tracker_id += 1; + } + } + else + { + // send terminated event + if (event_tracker[execute_tracker_id].on_failure) + { + event_tracker[execute_tracker_id].on_failure( + update_on_failure(event_tracker[execute_tracker_id].on_failure_param)); + execute_tracker_id += 1; + } + } + }; + // trigger the action client with goal auto goal_handle_future = action_client_->async_send_goal(goal_msg, send_goal_options_); @@ -184,29 +190,27 @@ class ActionRunner : public RunnerBase auto result_future = action_client_->async_get_result(goal_handle); - // create a function to call for the result - // the future will be returned to the caller - // and the caller can provide a conversion function - // to handle the result - std::function result_callback = - [this, result_future](tinyxml2::XMLElement* result) { - // wait for result - if (rclcpp::spin_until_future_complete(node_, result_future) != rclcpp::FutureReturnCode::SUCCESS) - { - RCLCPP_ERROR(node_->get_logger(), "get result call failed"); - return; - } + // create a function to call for the result. the future will be returned to the caller and the caller + // can provide a conversion function to handle the result + + std::function result_callback = [this, result_future](tinyxml2::XMLElement* result) { + // wait for result + if (rclcpp::spin_until_future_complete(node_, result_future) != rclcpp::FutureReturnCode::SUCCESS) + { + RCLCPP_ERROR(node_->get_logger(), "get result call failed"); + return; + } - // get wrapped result - typename rclcpp_action::ClientGoalHandle::WrappedResult wrapped_result = result_future.get(); + // get wrapped result + typename rclcpp_action::ClientGoalHandle::WrappedResult wrapped_result = result_future.get(); - // convert the result - if (wrapped_result.code == rclcpp_action::ResultCode::SUCCEEDED) - { - result = generate_result(wrapped_result.result); - return; - } - }; + // convert the result + if (wrapped_result.code == rclcpp_action::ResultCode::SUCCEEDED) + { + result = generate_result(wrapped_result.result); + return; + } + }; return result_callback; } diff --git a/capabilities2_runner/include/capabilities2_runner/encap_runner.hpp b/capabilities2_runner/include/capabilities2_runner/encap_runner.hpp index 37ef6e5..933b8cc 100644 --- a/capabilities2_runner/include/capabilities2_runner/encap_runner.hpp +++ b/capabilities2_runner/include/capabilities2_runner/encap_runner.hpp @@ -33,21 +33,15 @@ class EnCapRunner : public ActionRunner * this is deferred since the action client topic name is not known at this level * of abstraction * - * @param node - * @param run_config - * @param action_name - * @param on_started - * @param on_terminated - * @param on_stopped + * @param node shared pointer to the capabilities node. Allows to use ros node related functionalities + * @param run_config runner configuration loaded from the yaml file + * @param action_name action name used in the yaml file, used to load specific configuration from the run_config */ virtual void init_encapsulated_action(rclcpp::Node::SharedPtr node, const runner_opts& run_config, - const std::string& action_name, - std::function on_started = nullptr, - std::function on_terminated = nullptr, - std::function on_stopped = nullptr) + const std::string& action_name) { // init the base action runner - init_action(node, run_config, action_name, on_started, on_terminated, on_stopped); + init_action(node, run_config, action_name); // create an encapsulating action server encap_action_ = rclcpp_action::create_server( diff --git a/capabilities2_runner/include/capabilities2_runner/launch_runner.hpp b/capabilities2_runner/include/capabilities2_runner/launch_runner.hpp index 5faa18f..91d98e4 100644 --- a/capabilities2_runner/include/capabilities2_runner/launch_runner.hpp +++ b/capabilities2_runner/include/capabilities2_runner/launch_runner.hpp @@ -26,19 +26,11 @@ class LaunchRunner : public NoTriggerActionRunner on_started = nullptr, - std::function on_failure = nullptr, - std::function on_success = nullptr, - std::function on_stopped = nullptr) override + virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config) override { // store node pointer and run_config - init_action(node, run_config, "capabilities_launch_proxy/launch", on_started, on_failure, on_success, on_stopped); + init_action(node, run_config, "capabilities_launch_proxy/launch"); // get the package path from environment variable std::string package_path; @@ -64,6 +56,8 @@ class LaunchRunner : public NoTriggerActionRunnerasync_send_goal(goal, send_goal_options_); } diff --git a/capabilities2_runner/include/capabilities2_runner/runner_base.hpp b/capabilities2_runner/include/capabilities2_runner/runner_base.hpp index a7eaf1c..ce96694 100644 --- a/capabilities2_runner/include/capabilities2_runner/runner_base.hpp +++ b/capabilities2_runner/include/capabilities2_runner/runner_base.hpp @@ -58,6 +58,32 @@ struct runner_opts std::string pid; }; +/** + * @brief event options + * + * keeps track of events that are related to runner instances at various points of the + * plan + * @param on_started pointer to function to execute on start + * @param on_success pointer to function to execute on success + * @param on_failure pointer to function to execute on failure + * @param on_stopped pointer to function to execute on stop + * @param on_started_param parameters for the function to execute on start + * @param on_success_param parameters for the function to execute on success + * @param on_failure_param parameters for the function to execute on failure + * @param on_stopped_param parameters for the function to execute on stop + */ +struct event_opts +{ + std::function on_started; + std::function on_success; + std::function on_failure; + std::function on_stopped; + tinyxml2::XMLElement* on_started_param; + tinyxml2::XMLElement* on_success_param; + tinyxml2::XMLElement* on_failure_param; + tinyxml2::XMLElement* on_stopped_param; +}; + class RunnerBase { public: @@ -76,16 +102,8 @@ class RunnerBase * * @param node shared pointer to the capabilities node. Allows to use ros node related functionalities * @param run_config runner configuration loaded from the yaml file - * @param on_started pointer to function to execute on starting the runner - * @param on_failure pointer to function to execute on failure of the runner - * @param on_success pointer to function to execute on success of the runner - * @param on_stopped pointer to function to execute on stopping the runner */ - virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config, - std::function on_started = nullptr, - std::function on_failure = nullptr, - std::function on_success = nullptr, - std::function on_stopped = nullptr) = 0; + virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config) = 0; /** * @brief stop the runner @@ -112,22 +130,48 @@ class RunnerBase * * @param node shared pointer to the capabilities node. Allows to use ros node related functionalities * @param run_config runner configuration loaded from the yaml file - * @param on_started pointer to function to execute on starting the runner - * @param on_terminated pointer to function to execute on terminating the runner */ - void init_base(rclcpp::Node::SharedPtr node, const runner_opts& run_config, - std::function on_started = nullptr, - std::function on_failure = nullptr, - std::function on_success = nullptr, - std::function on_stopped = nullptr) + void init_base(rclcpp::Node::SharedPtr node, const runner_opts& run_config) { // store node pointer and opts node_ = node; run_config_ = run_config; - on_started_ = on_started; - on_failure_ = on_failure; - on_stopped_ = on_stopped; - on_success_ = on_success; + insert_tracker_id = 0; + execute_tracker_id = 0; + } + + /** + * @brief attach events to the runner + * + * @param on_started pointer to function to execute on starting the runner + * @param on_started_param parameters to triggers the on_started event with + * @param on_failure pointer to function to execute on failure of the runner + * @param on_failure_param parameters to triggers the on_started event with + * @param on_success pointer to function to execute on success of the runner + * @param on_success_param parameters to triggers the on_started event with + * @param on_stopped pointer to function to execute on stopping the runner + * @param on_stopped_param parameters to triggers the on_started event with + */ + void attach_events( + std::function on_started = nullptr, tinyxml2::XMLElement* on_started_param = nullptr, + std::function on_failure = nullptr, tinyxml2::XMLElement* on_failure_param = nullptr, + std::function on_success = nullptr, tinyxml2::XMLElement* on_success_param = nullptr, + std::function on_stopped = nullptr, tinyxml2::XMLElement* on_stopped_param = nullptr) + { + event_opts event; + + event.on_started = on_started; + event.on_failure = on_failure; + event.on_stopped = on_stopped; + event.on_success = on_success; + + event.on_started_param = on_started_param; + event.on_failure_param = on_failure_param; + event.on_success_param = on_success_param; + event.on_stopped_param = on_stopped_param; + + event_tracker[insert_tracker_id] = event; + insert_tracker_id += 1; } /** @@ -171,6 +215,71 @@ class RunnerBase } protected: + + /** + * @brief Update on_started event parameters with new data if avaible. + * + * This function is used to inject new data into the XMLElement containing + * parameters related to the on_started trigger event + * + * A pattern needs to be implemented in the derived class + * + * @param parameters pointer to the XMLElement containing parameters + * @return pointer to the XMLElement containing updated parameters + */ + virtual tinyxml2::XMLElement* update_on_started(tinyxml2::XMLElement* parameters) + { + return parameters; + }; + + /** + * @brief Update on_stopped event parameters with new data if avaible. + * + * This function is used to inject new data into the XMLElement containing + * parameters related to the on_stopped trigger event + * + * A pattern needs to be implemented in the derived class + * + * @param parameters pointer to the XMLElement containing parameters + * @return pointer to the XMLElement containing updated parameters + */ + virtual tinyxml2::XMLElement* update_on_stopped(tinyxml2::XMLElement* parameters) + { + return parameters; + }; + + /** + * @brief Update on_failure event parameters with new data if avaible. + * + * This function is used to inject new data into the XMLElement containing + * parameters related to the on_failure trigger event + * + * A pattern needs to be implemented in the derived class + * + * @param parameters pointer to the XMLElement containing parameters + * @return pointer to the XMLElement containing updated parameters + */ + virtual tinyxml2::XMLElement* update_on_failure(tinyxml2::XMLElement* parameters) + { + return parameters; + }; + + /** + * @brief Update on_success event parameters with new data if avaible. + * + * This function is used to inject new data into the XMLElement containing + * parameters related to the on_success trigger event + * + * A pattern needs to be implemented in the derived class + * + * @param parameters pointer to the XMLElement containing parameters + * @return pointer to the XMLElement containing updated parameters + */ + virtual tinyxml2::XMLElement* update_on_success(tinyxml2::XMLElement* parameters) + { + return parameters; + }; + // run config getters /** @@ -306,34 +415,29 @@ class RunnerBase protected: /** - * @param node shared pointer to the capabilities node. Allows to use ros node related functionalities + * @brief shared pointer to the capabilities node. Allows to use ros node related functionalities */ rclcpp::Node::SharedPtr node_; /** - * @param node runner configuration + * @brief run_config_ runner configuration */ runner_opts run_config_; /** - * @param node pointer to function to execute on starting the runner - */ - std::function on_started_; - - /** - * @param node pointer to function to execute on success + * @brief dictionary of events */ - std::function on_success_; + std::map event_tracker; /** - * @param node pointer to function to execute on failure + * @brief Last tracker id to be inserted */ - std::function on_failure_; + int insert_tracker_id; /** - * @param node pointer to function to execute on stopping the runner + * @brief Last tracker id to be executed */ - std::function on_stopped_; + int execute_tracker_id; }; } // namespace capabilities2_runner diff --git a/capabilities2_runner/src/standard_runners.cpp b/capabilities2_runner/src/standard_runners.cpp index 1e5616b..75b2f64 100644 --- a/capabilities2_runner/src/standard_runners.cpp +++ b/capabilities2_runner/src/standard_runners.cpp @@ -7,14 +7,10 @@ namespace capabilities2_runner class DummyRunner : public RunnerBase { public: - void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config, - std::function on_started = nullptr, - std::function on_failure = nullptr, - std::function on_success = nullptr, - std::function on_stopped = nullptr) override + void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config) override { // init the base runner - init_base(node, run_config, on_started, on_failure, on_success, on_stopped); + init_base(node, run_config); // do nothing } 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 e8f1f61..53f4693 100644 --- a/capabilities2_runner_audio/include/capabilities2_runner_audio/listen_runner.hpp +++ b/capabilities2_runner_audio/include/capabilities2_runner_audio/listen_runner.hpp @@ -30,18 +30,10 @@ class ListenerRunner : public ActionRunner * * @param node shared pointer to the capabilities node. Allows to use ros node related functionalities * @param run_config runner configuration loaded from the yaml file - * @param on_started pointer to function to execute on starting the runner - * @param on_failure pointer to function to execute on failure of the runner - * @param on_success pointer to function to execute on success of the runner - * @param on_stopped pointer to function to execute on stopping the runner */ - virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config, - std::function on_started = nullptr, - std::function on_failure = nullptr, - std::function on_success = nullptr, - std::function on_stopped = nullptr) override + virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config) override { - init_action(node, run_config, "speech_to_text", on_started, on_failure, on_success, on_stopped); + init_action(node, run_config, "speech_to_text"); } protected: diff --git a/capabilities2_runner_audio/include/capabilities2_runner_audio/speak_runner.hpp b/capabilities2_runner_audio/include/capabilities2_runner_audio/speak_runner.hpp index 98824a0..d8f37bc 100644 --- a/capabilities2_runner_audio/include/capabilities2_runner_audio/speak_runner.hpp +++ b/capabilities2_runner_audio/include/capabilities2_runner_audio/speak_runner.hpp @@ -30,18 +30,10 @@ class SpeakerRunner : public ActionRunner * * @param node shared pointer to the capabilities node. Allows to use ros node related functionalities * @param run_config runner configuration loaded from the yaml file - * @param on_started pointer to function to execute on starting the runner - * @param on_failure pointer to function to execute on failure of the runner - * @param on_success pointer to function to execute on success of the runner - * @param on_stopped pointer to function to execute on stopping the runner */ - virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config, - std::function on_started = nullptr, - std::function on_failure = nullptr, - std::function on_success = nullptr, - std::function on_stopped = nullptr) override + virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config) override { - init_action(node, run_config, "text_to_speech", on_started, on_failure, on_success, on_stopped); + init_action(node, run_config, "text_to_speech"); } 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 958ed93..aa781a2 100644 --- a/capabilities2_runner_nav2/include/capabilities2_runner_nav2/waypoint_runner.hpp +++ b/capabilities2_runner_nav2/include/capabilities2_runner_nav2/waypoint_runner.hpp @@ -32,18 +32,10 @@ class WayPointRunner : public ActionRunner * * @param node shared pointer to the capabilities node. Allows to use ros node related functionalities * @param run_config runner configuration loaded from the yaml file - * @param on_started pointer to function to execute on starting the runner - * @param on_failure pointer to function to execute on failure of the runner - * @param on_success pointer to function to execute on success of the runner - * @param on_stopped pointer to function to execute on stopping the runner */ - virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config, - std::function on_started = nullptr, - std::function on_failure = nullptr, - std::function on_success = nullptr, - std::function on_stopped = nullptr) override + virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config) override { - init_action(node, run_config, "follow_waypoints", on_started, on_failure, on_success, on_stopped); + init_action(node, run_config, "follow_waypoints"); } protected: From 9257b1680c21be13b0a065d5ab0578d06dbbca1f Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Wed, 23 Oct 2024 00:26:26 +1100 Subject: [PATCH 004/133] updated capabilities_server basic functionality --- .../capabilities_executor.hpp | 49 ++++++-- .../capabilities_xml_parser.hpp | 9 +- capabilities2_msgs/msg/Capability.msg | 2 + .../msg/CapabilityConnection.msg | 15 +-- .../capabilities2_runner/runner_base.hpp | 29 +---- .../capabilities2_server/capabilities_api.hpp | 55 +++++---- .../capabilities_server.hpp | 27 ++--- .../capabilities2_server/runner_cache.hpp | 113 ++++++++++++++++-- 8 files changed, 187 insertions(+), 112 deletions(-) diff --git a/capabilities2_executor/include/capabilities2_executor/capabilities_executor.hpp b/capabilities2_executor/include/capabilities2_executor/capabilities_executor.hpp index d956b61..1e9f445 100644 --- a/capabilities2_executor/include/capabilities2_executor/capabilities_executor.hpp +++ b/capabilities2_executor/include/capabilities2_executor/capabilities_executor.hpp @@ -342,54 +342,79 @@ class CapabilitiesExecutor : public rclcpp::Node { RCLCPP_INFO(this->get_logger(), "Node : %i", key); - if (capabilities2_xml_parser::convert_to_string(value.source.parameters, connection_msg.source_parameters)) + if (capabilities2_xml_parser::convert_to_string(value.source.parameters, connection_msg.source.parameters)) { connection_msg.source.capability = value.source.runner; connection_msg.source.provider = value.source.provider; RCLCPP_INFO(this->get_logger(), "Source Capability : %s", connection_msg.source.capability.c_str()); RCLCPP_INFO(this->get_logger(), "Source Provider : %s", connection_msg.source.provider.c_str()); - RCLCPP_INFO(this->get_logger(), "Source Parameters : %s", connection_msg.source_parameters.c_str()); + RCLCPP_INFO(this->get_logger(), "Source Parameters : %s", connection_msg.source.parameters.c_str()); + } + else + { + connection_msg.source.capability = ""; + connection_msg.source.provider = ""; } - if (capabilities2_xml_parser::convert_to_string(value.target_on_start.parameters, connection_msg.target_on_start_parameters)) + if (capabilities2_xml_parser::convert_to_string(value.target_on_start.parameters, connection_msg.target_on_start.parameters)) { connection_msg.target_on_start.capability = value.target_on_start.runner; connection_msg.target_on_start.provider = value.target_on_start.provider; RCLCPP_INFO(this->get_logger(), "Triggered on start Capability : %s", connection_msg.target_on_start.capability.c_str()); RCLCPP_INFO(this->get_logger(), "Triggered on start Provider : %s", connection_msg.target_on_start.provider.c_str()); - RCLCPP_INFO(this->get_logger(), "Triggered on start Parameters : %s", connection_msg.target_on_start_parameters.c_str()); + RCLCPP_INFO(this->get_logger(), "Triggered on start Parameters : %s", connection_msg.target_on_start.parameters.c_str()); + } + else + { + connection_msg.target_on_start.capability = ""; + connection_msg.target_on_start.provider = ""; } - if (capabilities2_xml_parser::convert_to_string(value.target_on_stop.parameters, connection_msg.target_on_stop_parameters)) + if (capabilities2_xml_parser::convert_to_string(value.target_on_stop.parameters, connection_msg.target_on_stop.parameters)) { connection_msg.target_on_stop.capability = value.target_on_stop.runner; connection_msg.target_on_stop.provider = value.target_on_stop.provider; RCLCPP_INFO(this->get_logger(), "Triggered on stop Capability : %s", connection_msg.target_on_stop.capability.c_str()); RCLCPP_INFO(this->get_logger(), "Triggered on stop Provider : %s", connection_msg.target_on_stop.provider.c_str()); - RCLCPP_INFO(this->get_logger(), "Triggered on stop Parameters : %s", connection_msg.target_on_stop_parameters.c_str()); + RCLCPP_INFO(this->get_logger(), "Triggered on stop Parameters : %s", connection_msg.target_on_stop.parameters.c_str()); + } + else + { + connection_msg.target_on_stop.capability = ""; + connection_msg.target_on_stop.provider = ""; } - if (capabilities2_xml_parser::convert_to_string(value.target_on_success.parameters, connection_msg.target_on_success_parameters)) + if (capabilities2_xml_parser::convert_to_string(value.target_on_success.parameters, connection_msg.target_on_success.parameters)) { connection_msg.target_on_success.capability = value.target_on_success.runner; connection_msg.target_on_success.provider = value.target_on_success.provider; RCLCPP_INFO(this->get_logger(), "Triggered on success Capability : %s", connection_msg.target_on_success.capability.c_str()); RCLCPP_INFO(this->get_logger(), "Triggered on success Provider : %s", connection_msg.target_on_success.provider.c_str()); - RCLCPP_INFO(this->get_logger(), "Triggered on success Parameters : %s", connection_msg.target_on_success_parameters.c_str()); + RCLCPP_INFO(this->get_logger(), "Triggered on success Parameters : %s", connection_msg.target_on_success.parameters.c_str()); + } + else + { + connection_msg.target_on_success.capability = ""; + connection_msg.target_on_success.provider = ""; } - if (capabilities2_xml_parser::convert_to_string(value.target_on_failure.parameters, connection_msg.target_on_failure_parameters)) + if (capabilities2_xml_parser::convert_to_string(value.target_on_failure.parameters, connection_msg.target_on_failure.parameters)) { connection_msg.target_on_failure.capability = value.target_on_failure.runner; connection_msg.target_on_failure.provider = value.target_on_failure.provider; RCLCPP_INFO(this->get_logger(), "Triggered on failure Capability : %s", connection_msg.target_on_failure.capability.c_str()); RCLCPP_INFO(this->get_logger(), "Triggered on failure Provider : %s", connection_msg.target_on_failure.provider.c_str()); - RCLCPP_INFO(this->get_logger(), "Triggered on failure Parameters : %s", connection_msg.target_on_failure_parameters.c_str()); + RCLCPP_INFO(this->get_logger(), "Triggered on failure Parameters : %s", connection_msg.target_on_failure.parameters.c_str()); + } + else + { + connection_msg.target_on_failure.capability = ""; + connection_msg.target_on_failure.provider = ""; } connection_goal_msg.connections.push_back(connection_msg); @@ -466,9 +491,9 @@ class CapabilitiesExecutor : public rclcpp::Node for (const auto& failed_connection : result.result->failed_connections) { - RCLCPP_ERROR(this->get_logger(), "Failed Events : %s", failed_connection.source_parameters.c_str()); + RCLCPP_ERROR(this->get_logger(), "Failed Events : %s", failed_connection.source.parameters.c_str()); - result_out->failed_elements.push_back(failed_connection.source_parameters); + result_out->failed_elements.push_back(failed_connection.source.parameters); } server_goal_handle->canceled(result_out); diff --git a/capabilities2_executor/include/capabilities2_executor/capabilities_xml_parser.hpp b/capabilities2_executor/include/capabilities2_executor/capabilities_xml_parser.hpp index 65ff5f6..e72318a 100644 --- a/capabilities2_executor/include/capabilities2_executor/capabilities_xml_parser.hpp +++ b/capabilities2_executor/include/capabilities2_executor/capabilities_xml_parser.hpp @@ -72,16 +72,14 @@ bool convert_to_string(tinyxml2::XMLElement* element, std::string& paramters) if (element) { tinyxml2::XMLPrinter printer; - element->Accept(&printer); - paramters = printer.CStr(); - return true; } else { - false; + paramters = ""; + return false; } } @@ -95,11 +93,8 @@ bool convert_to_string(tinyxml2::XMLElement* element, std::string& paramters) std::string convert_to_string(tinyxml2::XMLDocument& document) { tinyxml2::XMLPrinter printer; - document.Accept(&printer); - std::string value = printer.CStr(); - return value; } diff --git a/capabilities2_msgs/msg/Capability.msg b/capabilities2_msgs/msg/Capability.msg index bab67a8..73384f0 100644 --- a/capabilities2_msgs/msg/Capability.msg +++ b/capabilities2_msgs/msg/Capability.msg @@ -2,3 +2,5 @@ string capability # Used provider string provider +# trigger parameters +string parameters diff --git a/capabilities2_msgs/msg/CapabilityConnection.msg b/capabilities2_msgs/msg/CapabilityConnection.msg index 2b8861e..b350b5c 100644 --- a/capabilities2_msgs/msg/CapabilityConnection.msg +++ b/capabilities2_msgs/msg/CapabilityConnection.msg @@ -1,10 +1,5 @@ -capabilities2_msgs/Capability source -string source_parameters -capabilities2_msgs/Capability target_on_start -string target_on_start_parameters -capabilities2_msgs/Capability target_on_stop -string target_on_stop_parameters -capabilities2_msgs/Capability target_on_success -string target_on_success_parameters -capabilities2_msgs/Capability target_on_failure -string target_on_failure_parameters \ No newline at end of file +Capability source +Capability target_on_start +Capability target_on_stop +Capability target_on_success +Capability target_on_failure \ No newline at end of file diff --git a/capabilities2_runner/include/capabilities2_runner/runner_base.hpp b/capabilities2_runner/include/capabilities2_runner/runner_base.hpp index ce96694..a59fa2b 100644 --- a/capabilities2_runner/include/capabilities2_runner/runner_base.hpp +++ b/capabilities2_runner/include/capabilities2_runner/runner_base.hpp @@ -143,34 +143,11 @@ class RunnerBase /** * @brief attach events to the runner * - * @param on_started pointer to function to execute on starting the runner - * @param on_started_param parameters to triggers the on_started event with - * @param on_failure pointer to function to execute on failure of the runner - * @param on_failure_param parameters to triggers the on_started event with - * @param on_success pointer to function to execute on success of the runner - * @param on_success_param parameters to triggers the on_started event with - * @param on_stopped pointer to function to execute on stopping the runner - * @param on_stopped_param parameters to triggers the on_started event with + * @param event_option event_options related for the action */ - void attach_events( - std::function on_started = nullptr, tinyxml2::XMLElement* on_started_param = nullptr, - std::function on_failure = nullptr, tinyxml2::XMLElement* on_failure_param = nullptr, - std::function on_success = nullptr, tinyxml2::XMLElement* on_success_param = nullptr, - std::function on_stopped = nullptr, tinyxml2::XMLElement* on_stopped_param = nullptr) + void attach_events(capabilities2_runner::event_opts& event_option) { - event_opts event; - - event.on_started = on_started; - event.on_failure = on_failure; - event.on_stopped = on_stopped; - event.on_success = on_success; - - event.on_started_param = on_started_param; - event.on_failure_param = on_failure_param; - event.on_success_param = on_success_param; - event.on_stopped_param = on_stopped_param; - - event_tracker[insert_tracker_id] = event; + event_tracker[insert_tracker_id] = event_option; insert_tracker_id += 1; } diff --git a/capabilities2_server/include/capabilities2_server/capabilities_api.hpp b/capabilities2_server/include/capabilities2_server/capabilities_api.hpp index 3f4437c..33bf8d9 100644 --- a/capabilities2_server/include/capabilities2_server/capabilities_api.hpp +++ b/capabilities2_server/include/capabilities2_server/capabilities_api.hpp @@ -17,6 +17,8 @@ #include #include +#include +#include #include #include #include @@ -81,16 +83,8 @@ class CapabilitiesAPI * @param node ros node pointer of the ros server * @param capability capability name to be started * @param provider provider of the capability - * @param on_started pointer to function to execute on starting the runner - * @param on_failure pointer to function to execute on failure of the runner - * @param on_success pointer to function to execute on success of the runner - * @param on_stopped pointer to function to execute on stopping the runner */ - void start_capability(rclcpp::Node::SharedPtr node, const std::string& capability, const std::string& provider, - std::function on_started = nullptr, - std::function on_failure = nullptr, - std::function on_success = nullptr, - std::function on_stopped = nullptr) + void start_capability(rclcpp::Node::SharedPtr node, const std::string& capability, const std::string& provider) { // get the running model from the db models::running_model_t running = cap_db_->get_running(provider); @@ -118,7 +112,7 @@ class CapabilitiesAPI // TODO: consider the logic for multiple runners per capability try { - runner_cache_.add_runner(node, capability, run_config, on_started, on_failure, on_success, on_stopped); + runner_cache_.add_runner(node, capability, run_config); // log RCLCPP_INFO(node_logging_interface_ptr_->get_logger(), "started capability: %s with provider: %s", @@ -138,8 +132,8 @@ class CapabilitiesAPI * @param capability capability name to be started * @param provider provider of the capability */ - void start_dependents(rclcpp::Node::SharedPtr node, const std::string& bond_id, - const std::string& capability, const std::string& provider) + void start_dependencies(rclcpp::Node::SharedPtr node, const std::string& bond_id, const std::string& capability, + const std::string& provider) { // Create bond id bond_cache_.add_bond(capability, bond_id); @@ -169,7 +163,7 @@ class CapabilitiesAPI * @param capability * @param parameters */ - void trigger_capability(const std::string& capability, tinyxml2::XMLElement* parameters = nullptr) + void trigger_capability(const std::string& capability, const std::string& parameters) { // trigger the runner try @@ -276,31 +270,36 @@ class CapabilitiesAPI } /** - * @brief Connect two capabilities sequentially. This will create a bond ids for the requested instances and, build - * a event callback for sequential triggering and start the capabilities. + * @brief Set triggers for `on_success`, `on_failure`, `on_start`, `on_stop` events for a given capability * - * @param node ros node pointer of the ros server - * @param capability capability name to be started - * @param provider provider of the capability + * @param capability capability from where the events originate + * @param on_started_capability capability triggered by on_start event + * @param on_started_parameters parameters related to capability triggered by on_start event + * @param on_stopped_capability capability triggered by on_stop event + * @param on_stopped_parameters parameters related to capability triggered by on_stop event + * @param on_success_capability capability triggered by on_success event + * @param on_success_parameters parameters related to capability triggered by on_success event + * @param on_failure_capability capability triggered by on_failure event + * @param on_failure_parameters parameters related to capability triggered by on_failure event */ - void create_serial_connection(rclcpp::Node::SharedPtr node, const std::string& bond_id, - const std::string& source_capability, const std::string& source_provider, - const std::string& target_capability, const std::string& target_provider) + void set_triggers(const std::string& capability, const std::string& on_started_capability, + const std::string& on_started_parameters, const std::string& on_failure_capability, + const std::string& on_failure_parameters, const std::string& on_success_capability, + const std::string& on_success_parameters, const std::string& on_stopped_capability, + const std::string& on_stopped_parameters) { - - - try { - runner_cache_.add_runner(node, capability, cap_db_->get_run_config(provider), on_started, on_failure, on_success, on_stopped); + runner_cache_.set_runner_triggers(capability, on_started_capability, on_started_parameters, on_failure_capability, + on_failure_parameters, on_success_capability, on_success_parameters, + on_stopped_capability, on_stopped_parameters); // log - RCLCPP_INFO(node_logging_interface_ptr_->get_logger(), "started capability: %s with provider: %s", - capability.c_str(), provider.c_str()); + RCLCPP_INFO(node_logging_interface_ptr_->get_logger(), "setting triggers for capability: %s", capability.c_str()); } catch (const capabilities2_runner::runner_exception& e) { - RCLCPP_WARN(node_logging_interface_ptr_->get_logger(), "could not start runner: %s", e.what()); + RCLCPP_WARN(node_logging_interface_ptr_->get_logger(), "could not set triggers: %s", e.what()); } } diff --git a/capabilities2_server/include/capabilities2_server/capabilities_server.hpp b/capabilities2_server/include/capabilities2_server/capabilities_server.hpp index 2e6dd10..b81bd54 100644 --- a/capabilities2_server/include/capabilities2_server/capabilities_server.hpp +++ b/capabilities2_server/include/capabilities2_server/capabilities_server.hpp @@ -16,7 +16,6 @@ #include #include - #include #include #include @@ -180,14 +179,6 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI std::bind(&CapabilitiesServer::handle_cancel, this, std::placeholders::_1), std::bind(&CapabilitiesServer::handle_accepted, this, std::placeholders::_1)); - // create publishing event callbacks by binding the event publisher and event message callbacks - // handled by the API class and passed around to runners - // on started, stopped, and terminated lambdas binding event_pub_ - // init events system callbacks with lambdas - init_events([this](const std::string& cap) { event_pub_->publish(on_capability_started(cap)); }, - [this](const std::string& cap) { event_pub_->publish(on_capability_stopped(cap)); }, - [this](const std::string& cap) { event_pub_->publish(on_capability_terminated(cap)); }); - // log ready RCLCPP_INFO(get_logger(), "capabilities server started"); @@ -436,21 +427,23 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI // start all runners and interfaces that the connections depend on for (const auto& connection : goal->connections) { - start_dependents(shared_from_this(), goal->bond_id, connection.target.capability, connection.target.provider); + start_dependencies(shared_from_this(), goal->bond_id, connection.source.capability, connection.source.provider); } // establish relationships for all the connections received - for (auto connection = goal->connections.rbegin(); connection != goal->connections.rend(); ++connection) - { - if (*connection.source.capability != "start") - } - - // trigger starting connections for (const auto& connection : goal->connections) { - // if (connection.source.capability == "start") + set_triggers(connection.source.capability, connection.target_on_start.capability, + connection.target_on_start.parameters, connection.target_on_failure.capability, + connection.target_on_failure.parameters, connection.target_on_success.capability, + connection.target_on_success.parameters, connection.target_on_stop.capability, + connection.target_on_stop.parameters); } + auto first_node = goal->connections[0]; + + trigger_capability(first_node.source.capability, first_node.source.parameters); + // response is empty } diff --git a/capabilities2_server/include/capabilities2_server/runner_cache.hpp b/capabilities2_server/include/capabilities2_server/runner_cache.hpp index cc32427..e6e841e 100644 --- a/capabilities2_server/include/capabilities2_server/runner_cache.hpp +++ b/capabilities2_server/include/capabilities2_server/runner_cache.hpp @@ -41,17 +41,9 @@ class RunnerCache * @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 on_started pointer to function to execute on starting the runner - * @param on_failure pointer to function to execute on failure of the runner - * @param on_success pointer to function to execute on success of the runner - * @param on_stopped pointer to function to execute on stopping the runner */ void add_runner(rclcpp::Node::SharedPtr node, const std::string& capability, - const models::run_config_model_t& run_config, - std::function on_started = nullptr, - std::function on_failure = nullptr, - std::function on_success = nullptr, - std::function on_stopped = nullptr) + const models::run_config_model_t& run_config) { // if the runner exists then throw an error if (running(capability)) @@ -82,7 +74,7 @@ class RunnerCache } // start the runner - runner_cache_[capability]->start(node, run_config.to_runner_opts(), on_started, on_failure, on_success, on_stopped); + runner_cache_[capability]->start(node, run_config.to_runner_opts()); } /** @@ -93,12 +85,16 @@ class RunnerCache * @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, tinyxml2::XMLElement* parameters = nullptr) + void trigger_runner(const std::string& capability, const std::string& parameters) { + tinyxml2::XMLDocument doc; + doc.Parse(parameters.c_str()); + tinyxml2::XMLElement* xml_parameters = doc.FirstChildElement(); + // is the runner in the cache if (running(capability)) { - runner_cache_[capability]->trigger(parameters); + runner_cache_[capability]->trigger(xml_parameters); } else { @@ -106,6 +102,96 @@ class RunnerCache } } + /** + * @brief Set triggers for `on_success`, `on_failure`, `on_start`, `on_stop` events + * + * + * @param capability capability from where the events originate + * @param on_started_capability capability triggered by on_start event + * @param on_started_parameters parameters related to capability triggered by on_start event + * @param on_stopped_capability capability triggered by on_stop event + * @param on_stopped_parameters parameters related to capability triggered by on_stop event + * @param on_success_capability capability triggered by on_success event + * @param on_success_parameters parameters related to capability triggered by on_success event + * @param on_failure_capability capability triggered by on_failure event + * @param on_failure_parameters parameters related to capability triggered by on_failure event + */ + void set_runner_triggers(const std::string& capability, const std::string& on_started_capability, + const std::string& on_started_parameters, const std::string& on_failure_capability, + const std::string& on_failure_parameters, const std::string& on_success_capability, + const std::string& on_success_parameters, const std::string& on_stopped_capability, + const std::string& on_stopped_parameters) + { + capabilities2_runner::event_opts event_options; + + if (on_started_capability != "") + { + event_options.on_started = [this, &on_started_capability](tinyxml2::XMLElement* parameters) { + runner_cache_[on_started_capability]->trigger(parameters); + }; + + tinyxml2::XMLDocument doc; + doc.Parse(on_started_parameters.c_str()); + event_options.on_started_param = doc.FirstChildElement(); + } + else + { + event_options.on_started = nullptr; + event_options.on_started_param = nullptr; + } + + if (on_failure_capability != "") + { + event_options.on_failure = [this, &on_failure_capability](tinyxml2::XMLElement* parameters) { + runner_cache_[on_failure_capability]->trigger(parameters); + }; + + tinyxml2::XMLDocument doc; + doc.Parse(on_failure_parameters.c_str()); + event_options.on_failure_param = doc.FirstChildElement(); + } + else + { + event_options.on_failure = nullptr; + event_options.on_failure_param = nullptr; + } + + if (on_success_capability != "") + { + event_options.on_success = [this, &on_success_capability](tinyxml2::XMLElement* parameters) { + runner_cache_[on_success_capability]->trigger(parameters); + }; + + tinyxml2::XMLDocument doc; + doc.Parse(on_success_parameters.c_str()); + event_options.on_success_param = doc.FirstChildElement(); + } + else + { + event_options.on_success = nullptr; + event_options.on_success_param = nullptr; + } + + if (on_stopped_capability != "") + { + event_options.on_stopped = [this, &on_stopped_capability](tinyxml2::XMLElement* parameters) { + runner_cache_[on_stopped_capability]->trigger(parameters); + }; + + tinyxml2::XMLDocument doc; + doc.Parse(on_stopped_parameters.c_str()); + event_options.on_stopped_param = doc.FirstChildElement(); + } + else + { + event_options.on_stopped = nullptr; + event_options.on_stopped_param = nullptr; + } + + runner_cache_[capability]->attach_events(event_options); + event_cache_[capability] = event_options; + } + /** * @brief Remove a given runner * @@ -242,6 +328,9 @@ class RunnerCache // capability / provider specs -> runner std::map> runner_cache_; + // map events to capabilities + std::map event_cache_; + // runner plugin loader pluginlib::ClassLoader runner_loader_; From 6f7e8ea0e05a05d8a8be3db8ddd73fb85aa2bf19 Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Wed, 23 Oct 2024 13:13:11 +1100 Subject: [PATCH 005/133] completed updating capabilities server --- .../capabilities2_server/capabilities_api.hpp | 49 ++++++------------- .../capabilities_server.hpp | 40 +++++++++++++-- .../capabilities2_server/runner_cache.hpp | 35 ------------- 3 files changed, 50 insertions(+), 74 deletions(-) diff --git a/capabilities2_server/include/capabilities2_server/capabilities_api.hpp b/capabilities2_server/include/capabilities2_server/capabilities_api.hpp index 33bf8d9..f7a0f86 100644 --- a/capabilities2_server/include/capabilities2_server/capabilities_api.hpp +++ b/capabilities2_server/include/capabilities2_server/capabilities_api.hpp @@ -83,9 +83,14 @@ class CapabilitiesAPI * @param node ros node pointer of the ros server * @param capability capability name to be started * @param provider provider of the capability + * + * @return `true` if capability started successfully. else returns `false` */ - void start_capability(rclcpp::Node::SharedPtr node, const std::string& capability, const std::string& provider) + bool start_capability(rclcpp::Node::SharedPtr node, const std::string& capability, const std::string& provider) { + // return value + bool value = true; + // get the running model from the db models::running_model_t running = cap_db_->get_running(provider); @@ -99,7 +104,7 @@ class CapabilitiesAPI bind_dependency(run.interface); // add the runner to the cache - start_capability(node, run.interface, run.provider); + value = value and start_capability(node, run.interface, run.provider); } // get the provider specification for the capability @@ -117,43 +122,17 @@ class CapabilitiesAPI // log RCLCPP_INFO(node_logging_interface_ptr_->get_logger(), "started capability: %s with provider: %s", capability.c_str(), provider.c_str()); + + return value and true; } catch (const capabilities2_runner::runner_exception& e) { RCLCPP_WARN(node_logging_interface_ptr_->get_logger(), "could not start runner: %s", e.what()); - } - } - - /** - * @brief Start the dependencies of a capability. Internal function only. Do not used this function externally. - * - * @param node ros node pointer of the ros server - * @param bond_id bond_id for the capability - * @param capability capability name to be started - * @param provider provider of the capability - */ - void start_dependencies(rclcpp::Node::SharedPtr node, const std::string& bond_id, const std::string& capability, - const std::string& provider) - { - // Create bond id - bond_cache_.add_bond(capability, bond_id); - // get the running model from the db - models::running_model_t running = cap_db_->get_running(provider); - - // start all dependencies - // go through the running model and start the necessary dependencies - for (const auto& run : running.dependencies) - { - RCLCPP_INFO(node_logging_interface_ptr_->get_logger(), "found dependency: %s", run.interface.c_str()); - - // make an internal 'use' bond for the capability dependency - bind_dependency(run.interface); - - // add the runner to the cache - start_capability(node, run.interface, run.provider); + return false; } } + /** * @brief trigger a capability * @@ -258,15 +237,17 @@ class CapabilitiesAPI * @param capability capability name to be started * @param provider provider of the capability * @param bond_id bond_id for the capability + * + * @return `true` if capability started successfully. else returns `false` */ - void use_capability(rclcpp::Node::SharedPtr node, const std::string& capability, const std::string& provider, + bool use_capability(rclcpp::Node::SharedPtr node, const std::string& capability, const std::string& provider, const std::string& bond_id) { // add bond to cache for capability bond_cache_.add_bond(capability, bond_id); // start the capability with the provider - start_capability(node, capability, provider); + return start_capability(node, capability, provider); } /** diff --git a/capabilities2_server/include/capabilities2_server/capabilities_server.hpp b/capabilities2_server/include/capabilities2_server/capabilities_server.hpp index b81bd54..dab86ea 100644 --- a/capabilities2_server/include/capabilities2_server/capabilities_server.hpp +++ b/capabilities2_server/include/capabilities2_server/capabilities_server.hpp @@ -422,29 +422,59 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI void launch_fabric( const std::shared_ptr> goal_handle) { - const auto goal = goal_handle->get_goal(); + auto goal = goal_handle->get_goal(); + auto result = std::make_shared(); // start all runners and interfaces that the connections depend on for (const auto& connection : goal->connections) { - start_dependencies(shared_from_this(), goal->bond_id, connection.source.capability, connection.source.provider); + // start the capability with bond id. + if (use_capability(shared_from_this(), connection.source.capability, connection.source.provider, goal->bond_id)) + { + // capability started succeesfully + RCLCPP_INFO(get_logger(), "Capability started: %s", connection.source.capability.c_str()); + } + else + { + //capability failed. so add it to failed connections to be sent to the action client + result->failed_connections.push_back(connection); + RCLCPP_ERROR(get_logger(), "Capability failed: %s", connection.source.capability.c_str()); + } } - // establish relationships for all the connections received + // check if there are any failed connections + if (result->failed_connections.size() > 0) + { + // there are failed connections. so cancel the action server process. and let the action + // client know that about the failed connection + goal_handle->canceled(result); + + // free the capabilites that were started since action execution failed + for (const auto& connection : goal->connections) + { + free_capability(connection.source.capability, goal->bond_id); + RCLCPP_ERROR(get_logger(), "Capability freed due to failure: %s", connection.source.capability.c_str()); + } + } + + // establish relationships for all the connections received. for (const auto& connection : goal->connections) { set_triggers(connection.source.capability, connection.target_on_start.capability, connection.target_on_start.parameters, connection.target_on_failure.capability, - connection.target_on_failure.parameters, connection.target_on_success.capability, + connection.target_on_failure.parameters, connection.target_on_success.capability, connection.target_on_success.parameters, connection.target_on_stop.capability, connection.target_on_stop.parameters); } + // identify the first capability in the fabric auto first_node = goal->connections[0]; + // trigger the first node so fabric starts rippling trigger_capability(first_node.source.capability, first_node.source.parameters); - // response is empty + // return success and let the action client know that the fabric was launched. + goal_handle->succeed(result); } void load_capabilities(const std::string& package_path) diff --git a/capabilities2_server/include/capabilities2_server/runner_cache.hpp b/capabilities2_server/include/capabilities2_server/runner_cache.hpp index e6e841e..1ef2d04 100644 --- a/capabilities2_server/include/capabilities2_server/runner_cache.hpp +++ b/capabilities2_server/include/capabilities2_server/runner_cache.hpp @@ -293,35 +293,6 @@ class RunnerCache return runner_cache_.find(capability) != runner_cache_.end(); } - /** - * @brief Callback function for 'on_started' event - * - * @param cb callback function pointer - */ - // void set_on_started(std::function cb) - // { - // on_started = cb; - // } - - /** - * @brief Callback function for 'on_stopped' event - * - * @param cb callback function pointer - */ - // void set_on_stopped(std::function cb) - // { - // on_stopped = cb; - // } - - /** - * @brief Callback function for 'on_failure' event - * - * @param cb callback function pointer - */ - // void set_on_terminated(std::function cb) - // { - // on_failure = cb; - // } private: // map capability to running model @@ -333,12 +304,6 @@ class RunnerCache // runner plugin loader pluginlib::ClassLoader runner_loader_; - - // event callbacks - // std::function on_started; - // std::function on_stopped; - // std::function on_failure; - // std::function on_success; }; } // namespace capabilities2_server From a1c2db55957add15c8ce507259820409047405d3 Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Thu, 24 Oct 2024 16:34:09 +1100 Subject: [PATCH 006/133] added topic runner and service runner --- .../capabilities2_runner/action_runner.hpp | 5 +- .../multi_action_runner.hpp | 266 ------------------ .../capabilities2_runner/service_runner.hpp | 170 +++++++++++ .../capabilities2_runner/topic_runner.hpp | 158 +++++++++++ 4 files changed, 329 insertions(+), 270 deletions(-) delete mode 100644 capabilities2_runner/include/capabilities2_runner/multi_action_runner.hpp create mode 100644 capabilities2_runner/include/capabilities2_runner/service_runner.hpp create mode 100644 capabilities2_runner/include/capabilities2_runner/topic_runner.hpp diff --git a/capabilities2_runner/include/capabilities2_runner/action_runner.hpp b/capabilities2_runner/include/capabilities2_runner/action_runner.hpp index 47340bc..58962eb 100644 --- a/capabilities2_runner/include/capabilities2_runner/action_runner.hpp +++ b/capabilities2_runner/include/capabilities2_runner/action_runner.hpp @@ -4,6 +4,7 @@ #include #include #include +#include #include #include @@ -117,10 +118,6 @@ class ActionRunner : public RunnerBase virtual std::optional> trigger(tinyxml2::XMLElement* parameters = nullptr) override { - // the action is being triggered out of order - // if (!goal_handle_) - // throw runner_exception("cannot trigger action that was not started"); - // if parameters are not provided then cannot proceed if (!parameters) throw runner_exception("cannot trigger action without parameters"); diff --git a/capabilities2_runner/include/capabilities2_runner/multi_action_runner.hpp b/capabilities2_runner/include/capabilities2_runner/multi_action_runner.hpp deleted file mode 100644 index 1cbb1c1..0000000 --- a/capabilities2_runner/include/capabilities2_runner/multi_action_runner.hpp +++ /dev/null @@ -1,266 +0,0 @@ -#pragma once - -#include -#include -#include -#include -#include - -#include -#include -#include - -#include - -namespace capabilities2_runner -{ - -/** - * @brief templated struct to handle Action clients and their respective goal_handles - * - * @tparam ActionT action type - */ -template -struct ActionClientBundle -{ - std::shared_ptr> action_client; - typename rclcpp_action::ClientGoalHandle::SharedPtr goal_handle; - typename rclcpp_action::Client::SendGoalOptions send_goal_options; -}; - -/** - * @brief Multi action runner base class - * - * Create action clients to run multiple action based capabilities - * this class provides helpers to set up sequential and concurrent actions - */ -class MultiActionRunner : public RunnerBase -{ -public: - /** - * @brief Constructor which needs to be empty due to plugin semantics - */ - MultiActionRunner() : RunnerBase() - { - } - - /** - * @brief create action clients - * - * Create a new action client and store it in the map - * - * @param action_name action name used in the yaml file, used to load specific configuration from the run_config - */ - template - void init_action(const std::string& action_name) - { - typename rclcpp_action::ClientGoalHandle::SharedPtr goal_handle_; - typename rclcpp_action::Client::SendGoalOptions send_goal_options_; - - auto client_ = rclcpp_action::create_client(node_, action_name); - - // wait for action server - RCLCPP_INFO(node_->get_logger(), "%s waiting for action: %s", run_config_.interface.c_str(), action_name.c_str()); - - if (!client_->wait_for_action_server(std::chrono::seconds(3))) - { - RCLCPP_ERROR(node_->get_logger(), "%s failed to connect to action: %s", run_config_.interface.c_str(), - action_name.c_str()); - throw runner_exception("failed to connect to action server"); - } - - ActionClientBundle bundle{ client_, goal_handle_, send_goal_options_ }; - - action_clients_map_[action_name] = std::make_any>(bundle); - } - - /** - * @brief Deinitializer function for stopping an the action - * - * Stop the named action and delete the action client - * - * @param action_name action name used in the yaml file, used to load specific configuration from the run_config - */ - template - void deinit_action(const std::string& action_name) - { - auto bundle = std::any_cast>(action_clients_map_[action_name]); - - // if the node pointer is empty then throw an error - // this means that the runner was not started and is being used out of order - - if (!node_) - throw runner_exception("cannot stop runner that was not started"); - - // throw an error if the action client is null - // this can happen if the runner is not able to find the action resource - if (!bundle.action_client) - throw runner_exception("cannot stop runner action that was not started"); - - // stop runner using action client - if (bundle.goal_handle) - { - try - { - auto cancel_future = bundle.action_client->async_cancel_goal( - bundle.goal_handle, [this](action_msgs::srv::CancelGoal_Response::SharedPtr response) { - if (response->return_code != action_msgs::srv::CancelGoal_Response::ERROR_NONE) - { - // throw runner_exception("failed to cancel runner"); - } - - // publish event - if (on_stopped_) - { - on_stopped_(run_config_.interface); - } - }); - - // wait for action to be stopped. hold the thread for 2 seconds to help keep callbacks in scope - rclcpp::spin_until_future_complete(node_, cancel_future, std::chrono::seconds(2)); - } - catch (const rclcpp_action::exceptions::UnknownGoalHandleError& e) - { - RCLCPP_ERROR(node_->get_logger(), "failed to cancel goal: %s", e.what()); - throw runner_exception(e.what()); - } - } - - // delete action client - bundle.action_client.reset(); - action_clients_map_.erase(action_name); - } - - /** - * @brief Trigger function for calling and triggering an action. Non blocking implementation so result will not - * be returned. Use when action triggering is required and result message of the action is not required. - * - * @param action_name action name used in the yaml file, used to load specific configuration from the run_config - * @param goal_msg goal message to be sent to the action server - * - * @returns True for success of launching an action. False for failure to launching the action. - */ - template - bool trigger_action(const std::string& action_name, typename ActionT::Goal& goal_msg) - { - auto bundle = std::any_cast>(action_clients_map_[action_name]); - - // result callback - bundle.send_goal_options.result_callback = - [this](const typename rclcpp_action::ClientGoalHandle::WrappedResult& wrapped_result) { - if (wrapped_result.code != rclcpp_action::ResultCode::SUCCEEDED) - { - // send terminated event - if (on_terminated_) - { - on_terminated_(run_config_.interface); - } - } - }; - - auto goal_handle_future = bundle.action_client->async_send_goal(goal_msg, bundle.send_goal_options); - - if (rclcpp::spin_until_future_complete(node_, goal_handle_future) != rclcpp::FutureReturnCode::SUCCESS) - { - RCLCPP_ERROR(node_->get_logger(), "send goal call failed :("); - return false; - } - - typename rclcpp_action::ClientGoalHandle::SharedPtr goal_handle = goal_handle_future.get(); - - if (!goal_handle) - { - RCLCPP_ERROR(node_->get_logger(), "Goal was rejected by server"); - return false; - } - else - { - // publish event - if (on_started_) - on_started_(run_config_.interface); - - // store goal handle to be used with stop funtion - bundle.goal_handle = goal_handle; - return true; - } - - action_clients_map_[action_name] = std::make_any>(bundle); - } - - /** - * @brief Trigger function for calling and triggering an action. Blocking implementation so result will be returned. - * Use when result message of the action is required. - * - * @param action_name action name used in the yaml file, used to load specific configuration from the run_config - * @param goal_msg goal message to be sent to the action server - * @param result_msg result message returned by the action server upon completion - * - * @returns True for success of completing an action. False for failure to complete the action. - */ - template - bool trigger_action_wait(const std::string& action_name, typename ActionT::Goal& goal_msg, - typename ActionT::Result::SharedPtr result_msg) - { - auto bundle = std::any_cast>(action_clients_map_[action_name]); - - auto goal_handle_future = bundle.action_client->async_send_goal(goal_msg, bundle.send_goal_options); - - if (rclcpp::spin_until_future_complete(node_, goal_handle_future) != rclcpp::FutureReturnCode::SUCCESS) - { - RCLCPP_ERROR(node_->get_logger(), "send goal call failed :("); - return false; - } - - typename rclcpp_action::ClientGoalHandle::SharedPtr goal_handle = goal_handle_future.get(); - - if (!goal_handle) - { - RCLCPP_ERROR(node_->get_logger(), "Goal was rejected by server"); - return false; - } - else - { - // publish event - if (on_started_) - on_started_(run_config_.interface); - - // store goal handle to be used with stop funtion - bundle.goal_handle = goal_handle; - } - - // Wait for the server to be done with the goal - auto result_future = bundle.action_client->async_get_result(goal_handle); - - RCLCPP_INFO(node_->get_logger(), "Waiting for result"); - - if (rclcpp::spin_until_future_complete(node_, result_future) != rclcpp::FutureReturnCode::SUCCESS) - { - RCLCPP_ERROR(node_->get_logger(), "get result call failed :("); - return false; - } - - typename rclcpp_action::ClientGoalHandle::WrappedResult wrapped_result = result_future.get(); - - if (wrapped_result.code == rclcpp_action::ResultCode::SUCCEEDED) - { - result_msg = wrapped_result.result; - return true; - } - else - { - // send terminated event - if (on_terminated_) - on_terminated_(run_config_.interface); - return false; - } - } - -protected: - /** - * Dictionary to hold action client bundle. The key is a string, and the value is a - * polymorphic bundle. - * */ - std::map action_clients_map_; -}; - -} // namespace capabilities2_runner diff --git a/capabilities2_runner/include/capabilities2_runner/service_runner.hpp b/capabilities2_runner/include/capabilities2_runner/service_runner.hpp new file mode 100644 index 0000000..d0da108 --- /dev/null +++ b/capabilities2_runner/include/capabilities2_runner/service_runner.hpp @@ -0,0 +1,170 @@ + +#include + +#include "rclcpp/rclcpp.hpp" + +#include + +namespace capabilities2_runner +{ + +/** + * @brief service runner base class + * + * Create an server client to run an service based capability + */ +template +class ServiceRunner : public RunnerBase +{ +public: + /** + * @brief Constructor which needs to be empty due to plugin semantics + */ + ServiceRunner() : RunnerBase() + { + } + + /** + * @brief Initializer function for initializing the service runner in place of constructor due to plugin semantics + * + * @param node shared pointer to the capabilities node. Allows to use ros node related functionalities + * @param run_config runner configuration loaded from the yaml file + * @param service_name action name used in the yaml file, used to load specific configuration from the run_config + */ + virtual void init_service(rclcpp::Node::SharedPtr node, const runner_opts& run_config, + const std::string& service_name) + { + // initialize the runner base by storing node pointer and run config + init_base(node, run_config); + + // create an service client + service_client_ = node_->create_client(service_name); + + // wait for action server + RCLCPP_INFO(node_->get_logger(), "%s waiting for service: %s", run_config_.interface.c_str(), service_name.c_str()); + + if (!service_client_->wait_for_service(std::chrono::seconds(3))) + { + RCLCPP_ERROR(node_->get_logger(), "%s failed to connect to service: %s", run_config_.interface.c_str(), + service_name.c_str()); + throw runner_exception("failed to connect to server"); + } + } + + /** + * @brief the trigger function on the service runner is used to trigger an service. + * this method provides a mechanism for injecting parameters or a goal into a service + * and then trigger the service + * + * @param parameters + * @return std::optional> + */ + virtual std::optional> + trigger(tinyxml2::XMLElement* parameters = nullptr) override + { + // if parameters are not provided then cannot proceed + if (!parameters) + throw runner_exception("cannot trigger service without parameters"); + + // generate a goal from parameters if provided + typename ServiceT::Request request_msg = generate_request(parameters); + + auto result_future = service_client_->async_send_request(request_msg); + + if (event_tracker[execute_tracker_id].on_started) + { + event_tracker[execute_tracker_id].on_started( + update_on_started(event_tracker[execute_tracker_id].on_started_param)); + execute_tracker_id += 1; + } + + // create a function to call for the result. the future will be returned to the caller and the caller + // can provide a conversion function to handle the result + + std::function result_callback = [this, result_future](tinyxml2::XMLElement* result) { + if (rclcpp::spin_until_future_complete(node_, result_future) == rclcpp::FutureReturnCode::SUCCESS) + { + // send success event + if (event_tracker[execute_tracker_id].on_success) + { + event_tracker[execute_tracker_id].on_success( + update_on_success(event_tracker[execute_tracker_id].on_success_param)); + execute_tracker_id += 1; + } + + result = generate_response(result_future.get()); + } + else + { + RCLCPP_ERROR(node_->get_logger(), "get result call failed"); + + // send terminated event + if (event_tracker[execute_tracker_id].on_failure) + { + event_tracker[execute_tracker_id].on_failure( + update_on_failure(event_tracker[execute_tracker_id].on_failure_param)); + execute_tracker_id += 1; + } + } + }; + + return result_callback; + } + + /** + * @brief stop function to cease functionality and shutdown + * + */ + virtual void stop() override + { + // if the node pointer is empty then throw an error + // this means that the runner was not started and is being used out of order + + if (!node_) + throw runner_exception("cannot stop runner that was not started"); + + // throw an error if the service client is null + // this can happen if the runner is not able to find the action resource + + if (!service_client_) + throw runner_exception("cannot stop runner action that was not started"); + + // publish event + if (event_tracker[execute_tracker_id].on_stopped) + { + event_tracker[execute_tracker_id].on_stopped( + update_on_stopped(event_tracker[execute_tracker_id].on_stopped_param)); + execute_tracker_id += 1; + } + } + +protected: + /** + * @brief Generate a request from parameters + * + * This function is used in conjunction with the trigger function to inject type erased parameters + * into the typed action + * + * A pattern needs to be implemented in the derived class + * + * @param parameters + * @return ServiceT::Request the generated request + */ + virtual typename ServiceT::Request generate_request(tinyxml2::XMLElement* parameters) = 0; + + /** + * @brief generate a typed erased response + * + * this method is used in a callback passed to the trigger caller to get type erased result + * from the service the reponse can be passed by the caller or ignored + * + * The pattern needs to be implemented in the derived class + * + * @param wrapped_result + * @return tinyxml2::XMLElement* + */ + virtual tinyxml2::XMLElement* generate_response(const typename ServiceT::Result::SharedPtr& result) = 0; + + typename rclcpp::Client::SharedPtr service_client_; +}; +} // namespace capabilities2_runner \ No newline at end of file diff --git a/capabilities2_runner/include/capabilities2_runner/topic_runner.hpp b/capabilities2_runner/include/capabilities2_runner/topic_runner.hpp new file mode 100644 index 0000000..73bc766 --- /dev/null +++ b/capabilities2_runner/include/capabilities2_runner/topic_runner.hpp @@ -0,0 +1,158 @@ + +#include +#include + +#include "rclcpp/rclcpp.hpp" + +#include + +namespace capabilities2_runner +{ + +/** + * @brief Topic runner base class + * + * Create an topic subsriber for data grabbing capability + */ +template +class TopicRunner : public RunnerBase +{ +public: + /** + * @brief Constructor which needs to be empty due to plugin semantics + */ + TopicRunner() : RunnerBase() + { + } + + /** + * @brief Initializer function for initializing the topic runner in place of constructor due to plugin semantics + * + * @param node shared pointer to the capabilities node. Allows to use ros node related functionalities + * @param run_config runner configuration loaded from the yaml file + * @param topic_name topic name used in the yaml file, used to load specific configuration from the run_config + */ + virtual void init_subscriber(rclcpp::Node::SharedPtr node, const runner_opts& run_config, + const std::string& topic_name) + { + // initialize the runner base by storing node pointer and run config + init_base(node, run_config); + + // create an service client + subscription_ = node_->create_subscription(topic_name, 1, + std::bind(&TopicRunner::callback, this, std::placeholders::_1)); + } + + /** + * @brief the trigger function on the topic subscriber runner is used to trigger data retrival. + * + * @param parameters + * @return std::optional> + */ + virtual std::optional> + trigger(tinyxml2::XMLElement* parameters = nullptr) override + { + // if parameters are not provided then cannot proceed + if (!parameters) + throw runner_exception("cannot grab data without parameters"); + + if (event_tracker[execute_tracker_id].on_started) + { + event_tracker[execute_tracker_id].on_started( + update_on_started(event_tracker[execute_tracker_id].on_started_param)); + execute_tracker_id += 1; + } + + // create a function to call for the result. the future will be returned to the caller and the caller + // can provide a conversion function to handle the result + + std::function result_callback = [this](tinyxml2::XMLElement* result) { + if (!latest_message_) + { + // send success event + if (event_tracker[execute_tracker_id].on_success) + { + event_tracker[execute_tracker_id].on_success( + update_on_success(event_tracker[execute_tracker_id].on_success_param)); + execute_tracker_id += 1; + } + + // generate result + result = generate_message(latest_message_); + } + else + { + RCLCPP_ERROR(node_->get_logger(), "get result call failed"); + + // send terminated event + if (event_tracker[execute_tracker_id].on_failure) + { + event_tracker[execute_tracker_id].on_failure( + update_on_failure(event_tracker[execute_tracker_id].on_failure_param)); + execute_tracker_id += 1; + } + } + }; + + return result_callback; + } + + /** + * @brief stop function to cease functionality and shutdown + * + */ + virtual void stop() override + { + // if the node pointer is empty then throw an error + // this means that the runner was not started and is being used out of order + + if (!node_) + throw runner_exception("cannot stop runner that was not started"); + + // throw an error if the service client is null + // this can happen if the runner is not able to find the action resource + + if (!subscription_) + throw runner_exception("cannot stop runner subscriber that was not started"); + + // publish event + if (event_tracker[execute_tracker_id].on_stopped) + { + event_tracker[execute_tracker_id].on_stopped( + update_on_stopped(event_tracker[execute_tracker_id].on_stopped_param)); + execute_tracker_id += 1; + } + } + +protected: + /** + * @brief Callback to be executed when receiving a message + * + * This function is used grab the messages from the callback queue into a class + * parameter so that it can be used later on dering trigger + * + * @param msg message parameter + */ + void callback(const typename TopicT::SharedPtr msg) const + { + latest_message_ = msg; + } + + /** + * @brief generate xml data structure of the message + * + * this method is used in a callback passed to the trigger caller to get type erased result + * of the latest message can be passed by the caller or ignored + * + * The pattern needs to be implemented in the derived class + * + * @param wrapped_result + * @return tinyxml2::XMLElement* + */ + virtual tinyxml2::XMLElement* generate_message(const typename ServiceT::Result::SharedPtr& result) = 0; + + typename rclcpp::Subscription::SharedPtr subscription_; + + typename TopicT::SharedPtr latest_message_ = nullptr; +}; +} // namespace capabilities2_runner \ No newline at end of file From 96480d365e550aaa1cbd7022301e327ccb86666b Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Fri, 25 Oct 2024 16:10:53 +1100 Subject: [PATCH 007/133] added robotpose_runner and occupancygrid_runner --- .../capabilities2_runner/action_runner.hpp | 28 ++--- .../capabilities2_runner/runner_base.hpp | 15 ++- .../capabilities2_runner/service_runner.hpp | 62 +++++----- .../capabilities2_runner/topic_runner.hpp | 74 ++++++------ .../occupancygrid_runner.hpp | 113 ++++++++++++++++++ .../robotpose_runner.hpp | 94 +++++++++++++++ .../waypoint_runner.hpp | 2 +- capabilities2_runner_nav2/plugins.xml | 10 ++ .../src/nav2_runners.cpp | 4 + 9 files changed, 305 insertions(+), 97 deletions(-) create mode 100644 capabilities2_runner_nav2/include/capabilities2_runner_nav2/occupancygrid_runner.hpp create mode 100644 capabilities2_runner_nav2/include/capabilities2_runner_nav2/robotpose_runner.hpp diff --git a/capabilities2_runner/include/capabilities2_runner/action_runner.hpp b/capabilities2_runner/include/capabilities2_runner/action_runner.hpp index 58962eb..83d13f6 100644 --- a/capabilities2_runner/include/capabilities2_runner/action_runner.hpp +++ b/capabilities2_runner/include/capabilities2_runner/action_runner.hpp @@ -88,11 +88,10 @@ class ActionRunner : public RunnerBase } // publish event - if (event_tracker[execute_tracker_id].on_stopped) + if (events[execute_id].on_stopped) { - event_tracker[execute_tracker_id].on_stopped( - update_on_stopped(event_tracker[execute_tracker_id].on_stopped_param)); - execute_tracker_id += 1; + events[execute_id].on_stopped(update_on_stopped(events[execute_id].on_stopped_param)); + execute_id += 1; } }); @@ -131,11 +130,10 @@ class ActionRunner : public RunnerBase [this](const typename rclcpp_action::ClientGoalHandle::SharedPtr& goal_handle) { // publish event if (goal_handle) - if (event_tracker[execute_tracker_id].on_started) + if (events[execute_id].on_started) { - event_tracker[execute_tracker_id].on_started( - update_on_started(event_tracker[execute_tracker_id].on_started_param)); - execute_tracker_id += 1; + events[execute_id].on_started(update_on_started(events[execute_id].on_started_param)); + execute_id += 1; } // store goal handle to be used with stop funtion @@ -148,21 +146,19 @@ class ActionRunner : public RunnerBase if (wrapped_result.code == rclcpp_action::ResultCode::SUCCEEDED) { // send success event - if (event_tracker[execute_tracker_id].on_success) + if (events[execute_id].on_success) { - event_tracker[execute_tracker_id].on_success( - update_on_success(event_tracker[execute_tracker_id].on_success_param)); - execute_tracker_id += 1; + events[execute_id].on_success(update_on_success(events[execute_id].on_success_param)); + execute_id += 1; } } else { // send terminated event - if (event_tracker[execute_tracker_id].on_failure) + if (events[execute_id].on_failure) { - event_tracker[execute_tracker_id].on_failure( - update_on_failure(event_tracker[execute_tracker_id].on_failure_param)); - execute_tracker_id += 1; + events[execute_id].on_failure(update_on_failure(events[execute_id].on_failure_param)); + execute_id += 1; } } }; diff --git a/capabilities2_runner/include/capabilities2_runner/runner_base.hpp b/capabilities2_runner/include/capabilities2_runner/runner_base.hpp index a59fa2b..827e024 100644 --- a/capabilities2_runner/include/capabilities2_runner/runner_base.hpp +++ b/capabilities2_runner/include/capabilities2_runner/runner_base.hpp @@ -136,8 +136,8 @@ class RunnerBase // store node pointer and opts node_ = node; run_config_ = run_config; - insert_tracker_id = 0; - execute_tracker_id = 0; + insert_id = 0; + execute_id = 0; } /** @@ -147,8 +147,8 @@ class RunnerBase */ void attach_events(capabilities2_runner::event_opts& event_option) { - event_tracker[insert_tracker_id] = event_option; - insert_tracker_id += 1; + events[insert_id] = event_option; + insert_id += 1; } /** @@ -192,7 +192,6 @@ class RunnerBase } protected: - /** * @brief Update on_started event parameters with new data if avaible. * @@ -404,17 +403,17 @@ class RunnerBase /** * @brief dictionary of events */ - std::map event_tracker; + std::map events; /** * @brief Last tracker id to be inserted */ - int insert_tracker_id; + int insert_id; /** * @brief Last tracker id to be executed */ - int execute_tracker_id; + int execute_id; }; } // namespace capabilities2_runner diff --git a/capabilities2_runner/include/capabilities2_runner/service_runner.hpp b/capabilities2_runner/include/capabilities2_runner/service_runner.hpp index d0da108..692a0e1 100644 --- a/capabilities2_runner/include/capabilities2_runner/service_runner.hpp +++ b/capabilities2_runner/include/capabilities2_runner/service_runner.hpp @@ -1,4 +1,4 @@ - +#pragma once #include #include "rclcpp/rclcpp.hpp" @@ -71,41 +71,38 @@ class ServiceRunner : public RunnerBase auto result_future = service_client_->async_send_request(request_msg); - if (event_tracker[execute_tracker_id].on_started) + if (events[execute_id].on_started) { - event_tracker[execute_tracker_id].on_started( - update_on_started(event_tracker[execute_tracker_id].on_started_param)); - execute_tracker_id += 1; + events[execute_id].on_started(update_on_started(events[execute_id].on_started_param)); + execute_id += 1; } - // create a function to call for the result. the future will be returned to the caller and the caller - // can provide a conversion function to handle the result - - std::function result_callback = [this, result_future](tinyxml2::XMLElement* result) { - if (rclcpp::spin_until_future_complete(node_, result_future) == rclcpp::FutureReturnCode::SUCCESS) + if (rclcpp::spin_until_future_complete(node_, result_future) == rclcpp::FutureReturnCode::SUCCESS) + { + // send success event + if (events[execute_id].on_success) { - // send success event - if (event_tracker[execute_tracker_id].on_success) - { - event_tracker[execute_tracker_id].on_success( - update_on_success(event_tracker[execute_tracker_id].on_success_param)); - execute_tracker_id += 1; - } - - result = generate_response(result_future.get()); + events[execute_id].on_success(update_on_success(events[execute_id].on_success_param)); + execute_id += 1; } - else + } + else + { + RCLCPP_ERROR(node_->get_logger(), "get result call failed"); + + // send terminated event + if (events[execute_id].on_failure) { - RCLCPP_ERROR(node_->get_logger(), "get result call failed"); - - // send terminated event - if (event_tracker[execute_tracker_id].on_failure) - { - event_tracker[execute_tracker_id].on_failure( - update_on_failure(event_tracker[execute_tracker_id].on_failure_param)); - execute_tracker_id += 1; - } + events[execute_id].on_failure(update_on_failure(events[execute_id].on_failure_param)); + execute_id += 1; } + } + + // create a function to call for the result. the future will be returned to the caller and the caller + // can provide a conversion function to handle the result + + std::function result_callback = [this, result_future](tinyxml2::XMLElement* result) { + result = generate_response(result_future.get()); }; return result_callback; @@ -130,11 +127,10 @@ class ServiceRunner : public RunnerBase throw runner_exception("cannot stop runner action that was not started"); // publish event - if (event_tracker[execute_tracker_id].on_stopped) + if (events[execute_id].on_stopped) { - event_tracker[execute_tracker_id].on_stopped( - update_on_stopped(event_tracker[execute_tracker_id].on_stopped_param)); - execute_tracker_id += 1; + events[execute_id].on_stopped(update_on_stopped(events[execute_id].on_stopped_param)); + execute_id += 1; } } diff --git a/capabilities2_runner/include/capabilities2_runner/topic_runner.hpp b/capabilities2_runner/include/capabilities2_runner/topic_runner.hpp index 73bc766..aa1a147 100644 --- a/capabilities2_runner/include/capabilities2_runner/topic_runner.hpp +++ b/capabilities2_runner/include/capabilities2_runner/topic_runner.hpp @@ -1,4 +1,4 @@ - +#pragma once #include #include @@ -39,8 +39,9 @@ class TopicRunner : public RunnerBase init_base(node, run_config); // create an service client - subscription_ = node_->create_subscription(topic_name, 1, - std::bind(&TopicRunner::callback, this, std::placeholders::_1)); + subscription_ = + node_->create_subscription(topic_name, 1, + [this](const typename TopicT::SharedPtr msg) { this->callback(msg); }); } /** @@ -56,42 +57,38 @@ class TopicRunner : public RunnerBase if (!parameters) throw runner_exception("cannot grab data without parameters"); - if (event_tracker[execute_tracker_id].on_started) + if (events[execute_id].on_started) { - event_tracker[execute_tracker_id].on_started( - update_on_started(event_tracker[execute_tracker_id].on_started_param)); - execute_tracker_id += 1; + events[execute_id].on_started(update_on_started(events[execute_id].on_started_param)); + execute_id += 1; } - // create a function to call for the result. the future will be returned to the caller and the caller - // can provide a conversion function to handle the result - - std::function result_callback = [this](tinyxml2::XMLElement* result) { - if (!latest_message_) + if (!latest_message_) + { + // send success event + if (events[execute_id].on_success) { - // send success event - if (event_tracker[execute_tracker_id].on_success) - { - event_tracker[execute_tracker_id].on_success( - update_on_success(event_tracker[execute_tracker_id].on_success_param)); - execute_tracker_id += 1; - } - - // generate result - result = generate_message(latest_message_); + events[execute_id].on_success(update_on_success(events[execute_id].on_success_param)); + execute_id += 1; } - else + } + else + { + RCLCPP_ERROR(node_->get_logger(), "get result call failed"); + + // send terminated event + if (events[execute_id].on_failure) { - RCLCPP_ERROR(node_->get_logger(), "get result call failed"); - - // send terminated event - if (event_tracker[execute_tracker_id].on_failure) - { - event_tracker[execute_tracker_id].on_failure( - update_on_failure(event_tracker[execute_tracker_id].on_failure_param)); - execute_tracker_id += 1; - } + events[execute_id].on_failure(update_on_failure(events[execute_id].on_failure_param)); + execute_id += 1; } + } + + // create a function to call for the result. the future will be returned to the caller and the caller + // can provide a conversion function to handle the result + + std::function result_callback = [this](tinyxml2::XMLElement* result) { + result = generate_message(latest_message_); }; return result_callback; @@ -116,11 +113,10 @@ class TopicRunner : public RunnerBase throw runner_exception("cannot stop runner subscriber that was not started"); // publish event - if (event_tracker[execute_tracker_id].on_stopped) + if (events[execute_id].on_stopped) { - event_tracker[execute_tracker_id].on_stopped( - update_on_stopped(event_tracker[execute_tracker_id].on_stopped_param)); - execute_tracker_id += 1; + events[execute_id].on_stopped(update_on_stopped(events[execute_id].on_stopped_param)); + execute_id += 1; } } @@ -133,7 +129,7 @@ class TopicRunner : public RunnerBase * * @param msg message parameter */ - void callback(const typename TopicT::SharedPtr msg) const + void callback(const typename TopicT::SharedPtr& msg) const { latest_message_ = msg; } @@ -149,10 +145,10 @@ class TopicRunner : public RunnerBase * @param wrapped_result * @return tinyxml2::XMLElement* */ - virtual tinyxml2::XMLElement* generate_message(const typename ServiceT::Result::SharedPtr& result) = 0; + virtual tinyxml2::XMLElement* generate_message(typename TopicT::SharedPtr& result) = 0; typename rclcpp::Subscription::SharedPtr subscription_; - typename TopicT::SharedPtr latest_message_ = nullptr; + mutable typename TopicT::SharedPtr latest_message_ = nullptr; }; } // namespace capabilities2_runner \ No newline at end of file diff --git a/capabilities2_runner_nav2/include/capabilities2_runner_nav2/occupancygrid_runner.hpp b/capabilities2_runner_nav2/include/capabilities2_runner_nav2/occupancygrid_runner.hpp new file mode 100644 index 0000000..eb2183c --- /dev/null +++ b/capabilities2_runner_nav2/include/capabilities2_runner_nav2/occupancygrid_runner.hpp @@ -0,0 +1,113 @@ +#pragma once + +#include +#include + +#include + +#include + +namespace capabilities2_runner +{ + +/** + * @brief odometry runner class + * + * Capability Class to grab odometry data + * + */ +class OccupancyGridRunner : public TopicRunner +{ +public: + OccupancyGridRunner() : TopicRunner() + { + } + + /** + * @brief Starter function for starting the subscription runner + * + * @param node shared pointer to the capabilities node. Allows to use ros node related functionalities + * @param run_config runner configuration loaded from the yaml file + */ + virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config) override + { + init_subscriber(node, run_config, "map"); + } + +protected: + /** + * @brief This generate goal function overrides the generate_message() function from TopicRunner() + * @param result of the type geometry_msgs::msg::PoseWithCovarianceStamped + + * @return tinyxml2::XMLElement* of the robot's pose + */ + virtual tinyxml2::XMLElement* + generate_message(typename nav_msgs::msg::OccupancyGrid::SharedPtr& result) override + { + tinyxml2::XMLDocument* document = new tinyxml2::XMLDocument(); + tinyxml2::XMLNode* robot_pose = document->InsertEndChild(document->NewElement("OccupancyGrid")); + + tinyxml2::XMLElement* pose_element = document->NewElement("Info"); + + pose_element->SetAttribute("resolution", result->info.resolution); + pose_element->SetAttribute("width", result->info.width); + pose_element->SetAttribute("height", result->info.height); + pose_element->SetAttribute("origin.position.x", result->info.origin.position.x); + pose_element->SetAttribute("origin.position.y", result->info.origin.position.y); + pose_element->SetAttribute("origin.position.z", result->info.origin.position.z); + pose_element->SetAttribute("origin.orientation.x", result->info.origin.orientation.x); + pose_element->SetAttribute("origin.orientation.y", result->info.origin.orientation.y); + pose_element->SetAttribute("origin.orientation.z", result->info.origin.orientation.z); + pose_element->SetAttribute("origin.orientation.x", result->info.origin.orientation.w); + + tinyxml2::XMLElement* data_element = document->NewElement("Data"); + + std::string data_text = ""; + + for (const auto& data : result->data) + data_text += std::to_string(data) + " "; + + data_element->SetAttribute("data", data_text.c_str()); + + robot_pose->InsertFirstChild(pose_element); + robot_pose->InsertEndChild(data_element); + + return document->FirstChildElement("OccupancyGrid"); + } + + /** + * @brief Update on_success event parameters with new data if avaible. + * + * This function is used to inject new data into the XMLElement containing + * parameters related to the on_success trigger event + * + * A pattern needs to be implemented in the derived class + * + * @param parameters pointer to the XMLElement containing parameters + * @return pointer to the XMLElement containing updated parameters + */ + virtual tinyxml2::XMLElement* update_on_success(tinyxml2::XMLElement* parameters) + { + std::string data_text = ""; + + for (const auto& data : latest_message_->data) + data_text += std::to_string(data) + " "; + + parameters->SetAttribute("type", "nav_msgs/OccupancyGrid"); + + parameters->SetAttribute("info.resolution", latest_message_->info.resolution); + parameters->SetAttribute("info.width", latest_message_->info.width); + parameters->SetAttribute("info.height", latest_message_->info.height); + parameters->SetAttribute("info.origin.position.x", latest_message_->info.origin.position.x); + parameters->SetAttribute("info.origin.position.y", latest_message_->info.origin.position.y); + parameters->SetAttribute("info.origin.position.z", latest_message_->info.origin.position.z); + parameters->SetAttribute("info.origin.orientation.x", latest_message_->info.origin.orientation.x); + parameters->SetAttribute("info.origin.orientation.y", latest_message_->info.origin.orientation.y); + parameters->SetAttribute("info.origin.orientation.z", latest_message_->info.origin.orientation.z); + parameters->SetAttribute("info.origin.orientation.x", latest_message_->info.origin.orientation.w); + parameters->SetAttribute("data", data_text.c_str()); + + return parameters; + }; +}; +} // namespace capabilities2_runner \ No newline at end of file diff --git a/capabilities2_runner_nav2/include/capabilities2_runner_nav2/robotpose_runner.hpp b/capabilities2_runner_nav2/include/capabilities2_runner_nav2/robotpose_runner.hpp new file mode 100644 index 0000000..7b17de9 --- /dev/null +++ b/capabilities2_runner_nav2/include/capabilities2_runner_nav2/robotpose_runner.hpp @@ -0,0 +1,94 @@ +#pragma once + +#include +#include + +#include + +#include + +namespace capabilities2_runner +{ + +/** + * @brief odometry runner class + * + * Capability Class to grab odometry data + * + */ +class RobotPoseRunner : public TopicRunner +{ +public: + RobotPoseRunner() : TopicRunner() + { + } + + /** + * @brief Starter function for starting the subscription runner + * + * @param node shared pointer to the capabilities node. Allows to use ros node related functionalities + * @param run_config runner configuration loaded from the yaml file + */ + virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config) override + { + init_subscriber(node, run_config, "pose"); + } + +protected: + /** + * @brief This generate goal function overrides the generate_message() function from TopicRunner() + * @param result of the type geometry_msgs::msg::PoseWithCovarianceStamped + + * @return tinyxml2::XMLElement* of the robot's pose + */ + virtual tinyxml2::XMLElement* + generate_message(typename geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr& result) override + { + tinyxml2::XMLDocument* document = new tinyxml2::XMLDocument(); + tinyxml2::XMLNode* robot_pose = document->InsertEndChild(document->NewElement("RobotPose")); + + tinyxml2::XMLElement* pose_element = document->NewElement("Pose"); + + pose_element->SetAttribute("x", result->pose.pose.position.x); + pose_element->SetAttribute("y", result->pose.pose.position.y); + pose_element->SetAttribute("z", result->pose.pose.position.z); + + tinyxml2::XMLElement* orie_element = document->NewElement("Orientation"); + + orie_element->SetAttribute("x", result->pose.pose.orientation.x); + orie_element->SetAttribute("y", result->pose.pose.orientation.y); + orie_element->SetAttribute("z", result->pose.pose.orientation.z); + orie_element->SetAttribute("w", result->pose.pose.orientation.w); + + robot_pose->InsertFirstChild(pose_element); + robot_pose->InsertEndChild(orie_element); + + return document->FirstChildElement("RobotPose"); + } + + /** + * @brief Update on_success event parameters with new data if avaible. + * + * This function is used to inject new data into the XMLElement containing + * parameters related to the on_success trigger event + * + * A pattern needs to be implemented in the derived class + * + * @param parameters pointer to the XMLElement containing parameters + * @return pointer to the XMLElement containing updated parameters + */ + virtual tinyxml2::XMLElement* update_on_success(tinyxml2::XMLElement* parameters) + { + parameters->SetAttribute("type", "geometry_msgs/Pose"); + parameters->SetAttribute("position.x", latest_message_->pose.pose.position.x); + parameters->SetAttribute("position.y", latest_message_->pose.pose.position.y); + parameters->SetAttribute("position.z", latest_message_->pose.pose.position.z); + parameters->SetAttribute("orientation.x", latest_message_->pose.pose.orientation.x); + parameters->SetAttribute("orientation.y", latest_message_->pose.pose.orientation.y); + parameters->SetAttribute("orientation.z", latest_message_->pose.pose.orientation.z); + parameters->SetAttribute("orientation.x", latest_message_->pose.pose.orientation.w); + + return parameters; + }; +}; +} // namespace capabilities2_runner \ No newline at end of file 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 aa781a2..2c00796 100644 --- a/capabilities2_runner_nav2/include/capabilities2_runner_nav2/waypoint_runner.hpp +++ b/capabilities2_runner_nav2/include/capabilities2_runner_nav2/waypoint_runner.hpp @@ -15,7 +15,7 @@ namespace capabilities2_runner { /** - * @brief action runner base class + * @brief waypoint runner class * * Class to run waypointfollower action based capability * diff --git a/capabilities2_runner_nav2/plugins.xml b/capabilities2_runner_nav2/plugins.xml index a0d5780..d72c114 100644 --- a/capabilities2_runner_nav2/plugins.xml +++ b/capabilities2_runner_nav2/plugins.xml @@ -4,4 +4,14 @@ A plugin that provide nav2 waypoint navigation capability + + + A plugin that provide robot pose extraction capability + + + + + A plugin that provide occupancy grid map extraction capability + + diff --git a/capabilities2_runner_nav2/src/nav2_runners.cpp b/capabilities2_runner_nav2/src/nav2_runners.cpp index a457865..c5387c5 100644 --- a/capabilities2_runner_nav2/src/nav2_runners.cpp +++ b/capabilities2_runner_nav2/src/nav2_runners.cpp @@ -1,6 +1,8 @@ #include #include #include +#include +#include namespace capabilities2_runner { @@ -9,3 +11,5 @@ namespace capabilities2_runner // register runner plugins PLUGINLIB_EXPORT_CLASS(capabilities2_runner::WayPointRunner, capabilities2_runner::RunnerBase) +PLUGINLIB_EXPORT_CLASS(capabilities2_runner::RobotPoseRunner, capabilities2_runner::RunnerBase) +PLUGINLIB_EXPORT_CLASS(capabilities2_runner::OccupancyGridRunner, capabilities2_runner::RunnerBase) From e96802cff88e3d0f49b663a0d58310d2dee93963 Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Mon, 28 Oct 2024 13:36:33 +1100 Subject: [PATCH 008/133] added yaml files for capabilities2_runner_nav2 package --- .../occupancygrid_runner.hpp | 4 ++-- .../robotpose_runner.hpp | 2 +- capabilities2_runner_nav2/interfaces/map.yaml | 17 ++++++++++++++++ .../interfaces/pose.yaml | 17 ++++++++++++++++ capabilities2_runner_nav2/package.xml | 20 +++++++++++++++++++ .../providers/OccupancyGridRunner.yaml | 9 +++++++++ .../providers/RobotPoseRunner.yaml | 9 +++++++++ 7 files changed, 75 insertions(+), 3 deletions(-) create mode 100644 capabilities2_runner_nav2/interfaces/map.yaml create mode 100644 capabilities2_runner_nav2/interfaces/pose.yaml create mode 100644 capabilities2_runner_nav2/providers/OccupancyGridRunner.yaml create mode 100644 capabilities2_runner_nav2/providers/RobotPoseRunner.yaml diff --git a/capabilities2_runner_nav2/include/capabilities2_runner_nav2/occupancygrid_runner.hpp b/capabilities2_runner_nav2/include/capabilities2_runner_nav2/occupancygrid_runner.hpp index eb2183c..95f4a9a 100644 --- a/capabilities2_runner_nav2/include/capabilities2_runner_nav2/occupancygrid_runner.hpp +++ b/capabilities2_runner_nav2/include/capabilities2_runner_nav2/occupancygrid_runner.hpp @@ -13,7 +13,7 @@ namespace capabilities2_runner /** * @brief odometry runner class * - * Capability Class to grab odometry data + * Capability Class to grab occupancy grid data * */ class OccupancyGridRunner : public TopicRunner @@ -93,7 +93,7 @@ class OccupancyGridRunner : public TopicRunner for (const auto& data : latest_message_->data) data_text += std::to_string(data) + " "; - parameters->SetAttribute("type", "nav_msgs/OccupancyGrid"); + parameters->SetAttribute("type", "nav_msgs/msgs/OccupancyGrid"); parameters->SetAttribute("info.resolution", latest_message_->info.resolution); parameters->SetAttribute("info.width", latest_message_->info.width); diff --git a/capabilities2_runner_nav2/include/capabilities2_runner_nav2/robotpose_runner.hpp b/capabilities2_runner_nav2/include/capabilities2_runner_nav2/robotpose_runner.hpp index 7b17de9..ac5fe66 100644 --- a/capabilities2_runner_nav2/include/capabilities2_runner_nav2/robotpose_runner.hpp +++ b/capabilities2_runner_nav2/include/capabilities2_runner_nav2/robotpose_runner.hpp @@ -79,7 +79,7 @@ class RobotPoseRunner : public TopicRunnerSetAttribute("type", "geometry_msgs/Pose"); + parameters->SetAttribute("type", "geometry_msgs/msgs/Pose"); parameters->SetAttribute("position.x", latest_message_->pose.pose.position.x); parameters->SetAttribute("position.y", latest_message_->pose.pose.position.y); parameters->SetAttribute("position.z", latest_message_->pose.pose.position.z); diff --git a/capabilities2_runner_nav2/interfaces/map.yaml b/capabilities2_runner_nav2/interfaces/map.yaml new file mode 100644 index 0000000..e367948 --- /dev/null +++ b/capabilities2_runner_nav2/interfaces/map.yaml @@ -0,0 +1,17 @@ +# standardised nav2 interface specification +%YAML 1.1 +--- +name: map +spec_version: 1 +spec_type: interface +description: "data extraction capability to extract occupancy grid map of the robot" +interface: + topics: + "map": + type: "nav_msgs::msg::OccupancyGrid" + description: "This system allows the extraction of the occupancy grid map produced by the robot's mapping or localization system. + which enables the decision making authority to understand the environment around the robot. This can be triggered with a + '' command. This generally needs to be followed by with a prompting tool + event that allows for transfer of data to the decision making authority. This runner will populate the prompting tool's + execution parameters with the relavent data. As an example if this is followed by prompt_message runner provided by + PromptToolsRunner provider, such as ''. \ No newline at end of file diff --git a/capabilities2_runner_nav2/interfaces/pose.yaml b/capabilities2_runner_nav2/interfaces/pose.yaml new file mode 100644 index 0000000..02cd7d7 --- /dev/null +++ b/capabilities2_runner_nav2/interfaces/pose.yaml @@ -0,0 +1,17 @@ +# standardised nav2 interface specification +%YAML 1.1 +--- +name: pose +spec_version: 1 +spec_type: interface +description: "data extraction capability to extract pose of the robot" +interface: + topics: + "pose": + type: "geometry_msgs::msg::PoseWithCovarianceStamped" + description: "This system allows the extraction of the pose of the robot from the odoemtry or localization system. which enables + the decision making authority to understand the position and orientation of the robot. This can be triggered with a + '' command. This generally needs to be followed by with a prompting tool + event that allows for transfer of data to the decision making authority. This runner will populate the prompting tool's + execution parameters with the relavent data. As an example if this is followed by prompt_message runner provided by + PromptToolsRunner provider, such as '' \ No newline at end of file diff --git a/capabilities2_runner_nav2/package.xml b/capabilities2_runner_nav2/package.xml index 50b6505..d162138 100644 --- a/capabilities2_runner_nav2/package.xml +++ b/capabilities2_runner_nav2/package.xml @@ -31,7 +31,27 @@ interfaces/follow_waypoints.yaml + + + interfaces/map.yaml + + + + + interfaces/pose.yaml + + + + providers/RobotPoseRunner.yaml + + + + + providers/OccupancyGridRunner.yaml + + + providers/WaypointRunner.yaml diff --git a/capabilities2_runner_nav2/providers/OccupancyGridRunner.yaml b/capabilities2_runner_nav2/providers/OccupancyGridRunner.yaml new file mode 100644 index 0000000..35a2bce --- /dev/null +++ b/capabilities2_runner_nav2/providers/OccupancyGridRunner.yaml @@ -0,0 +1,9 @@ +# the empty provider +%YAML 1.1 +--- +name: OccupancyGridRunner +spec_type: provider +spec_version: 1.1 +description: The capability provider for the robot's occupancy grid map interface +implements: capabilities2_runner_nav2/OccupancyGridRunner +runner: capabilities2_runner::OccupancyGridRunner diff --git a/capabilities2_runner_nav2/providers/RobotPoseRunner.yaml b/capabilities2_runner_nav2/providers/RobotPoseRunner.yaml new file mode 100644 index 0000000..6de76c0 --- /dev/null +++ b/capabilities2_runner_nav2/providers/RobotPoseRunner.yaml @@ -0,0 +1,9 @@ +# the empty provider +%YAML 1.1 +--- +name: RobotPoseRunner +spec_type: provider +spec_version: 1.1 +description: The capability provider for the robot's odometry interface +implements: capabilities2_runner_nav2/RobotPoseRunner +runner: capabilities2_runner::RobotPoseRunner From 9c9c11e3843a5c3a9268880e5805ff519e253b1c Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Mon, 28 Oct 2024 17:16:21 +1100 Subject: [PATCH 009/133] started working on the capabilities2_runner_prompt --- .../capabilities2_runner/action_runner.hpp | 3 - .../capabilities2_runner/runner_base.hpp | 5 + .../capabilities2_runner/service_runner.hpp | 13 +-- capabilities2_runner_prompt/.clang-format | 64 +++++++++++++ capabilities2_runner_prompt/CMakeLists.txt | 71 ++++++++++++++ .../prompt_runner.hpp | 94 +++++++++++++++++++ .../interfaces/prompt.yaml | 19 ++++ capabilities2_runner_prompt/package.xml | 38 ++++++++ capabilities2_runner_prompt/plugins.xml | 7 ++ .../providers/PromptRunner.yaml | 9 ++ .../src/prompt_runners.cpp | 11 +++ 11 files changed, 325 insertions(+), 9 deletions(-) create mode 100644 capabilities2_runner_prompt/.clang-format create mode 100644 capabilities2_runner_prompt/CMakeLists.txt create mode 100644 capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_runner.hpp create mode 100644 capabilities2_runner_prompt/interfaces/prompt.yaml create mode 100644 capabilities2_runner_prompt/package.xml create mode 100644 capabilities2_runner_prompt/plugins.xml create mode 100644 capabilities2_runner_prompt/providers/PromptRunner.yaml create mode 100644 capabilities2_runner_prompt/src/prompt_runners.cpp diff --git a/capabilities2_runner/include/capabilities2_runner/action_runner.hpp b/capabilities2_runner/include/capabilities2_runner/action_runner.hpp index 83d13f6..f738617 100644 --- a/capabilities2_runner/include/capabilities2_runner/action_runner.hpp +++ b/capabilities2_runner/include/capabilities2_runner/action_runner.hpp @@ -235,9 +235,6 @@ class ActionRunner : public RunnerBase */ virtual tinyxml2::XMLElement* generate_result(const typename ActionT::Result::SharedPtr& result) = 0; - /**< pointer to XMLElement which contain parameters */ - tinyxml2::XMLElement* parameters_; - /**< action client */ typename rclcpp_action::Client::SharedPtr action_client_; diff --git a/capabilities2_runner/include/capabilities2_runner/runner_base.hpp b/capabilities2_runner/include/capabilities2_runner/runner_base.hpp index 827e024..42913f1 100644 --- a/capabilities2_runner/include/capabilities2_runner/runner_base.hpp +++ b/capabilities2_runner/include/capabilities2_runner/runner_base.hpp @@ -414,6 +414,11 @@ class RunnerBase * @brief Last tracker id to be executed */ int execute_id; + + /** + * @brief pointer to XMLElement which contain parameters + * */ + tinyxml2::XMLElement* parameters_; }; } // namespace capabilities2_runner diff --git a/capabilities2_runner/include/capabilities2_runner/service_runner.hpp b/capabilities2_runner/include/capabilities2_runner/service_runner.hpp index 692a0e1..07e4cf1 100644 --- a/capabilities2_runner/include/capabilities2_runner/service_runner.hpp +++ b/capabilities2_runner/include/capabilities2_runner/service_runner.hpp @@ -67,7 +67,7 @@ class ServiceRunner : public RunnerBase throw runner_exception("cannot trigger service without parameters"); // generate a goal from parameters if provided - typename ServiceT::Request request_msg = generate_request(parameters); + auto request_msg = std::make_shared(generate_request(parameters)); auto result_future = service_client_->async_send_request(request_msg); @@ -101,8 +101,9 @@ class ServiceRunner : public RunnerBase // create a function to call for the result. the future will be returned to the caller and the caller // can provide a conversion function to handle the result - std::function result_callback = [this, result_future](tinyxml2::XMLElement* result) { - result = generate_response(result_future.get()); + std::function result_callback = [this, &result_future](tinyxml2::XMLElement* result) mutable { + auto response = result_future.get(); + result = generate_response(response); }; return result_callback; @@ -139,7 +140,7 @@ class ServiceRunner : public RunnerBase * @brief Generate a request from parameters * * This function is used in conjunction with the trigger function to inject type erased parameters - * into the typed action + * into the typed service * * A pattern needs to be implemented in the derived class * @@ -151,7 +152,7 @@ class ServiceRunner : public RunnerBase /** * @brief generate a typed erased response * - * this method is used in a callback passed to the trigger caller to get type erased result + * This method is used in a callback passed to the trigger caller to get type erased result * from the service the reponse can be passed by the caller or ignored * * The pattern needs to be implemented in the derived class @@ -159,7 +160,7 @@ class ServiceRunner : public RunnerBase * @param wrapped_result * @return tinyxml2::XMLElement* */ - virtual tinyxml2::XMLElement* generate_response(const typename ServiceT::Result::SharedPtr& result) = 0; + virtual tinyxml2::XMLElement* generate_response(const typename ServiceT::Response::SharedPtr& result) const = 0; typename rclcpp::Client::SharedPtr service_client_; }; diff --git a/capabilities2_runner_prompt/.clang-format b/capabilities2_runner_prompt/.clang-format new file mode 100644 index 0000000..d36804f --- /dev/null +++ b/capabilities2_runner_prompt/.clang-format @@ -0,0 +1,64 @@ + +BasedOnStyle: Google +AccessModifierOffset: -2 +ConstructorInitializerIndentWidth: 2 +AlignEscapedNewlinesLeft: false +AlignTrailingComments: true +AllowAllParametersOfDeclarationOnNextLine: false +AllowShortIfStatementsOnASingleLine: false +AllowShortLoopsOnASingleLine: false +AllowShortFunctionsOnASingleLine: None +AlwaysBreakTemplateDeclarations: true +AlwaysBreakBeforeMultilineStrings: false +BreakBeforeBinaryOperators: false +BreakBeforeTernaryOperators: false +BreakConstructorInitializersBeforeComma: true +BinPackParameters: true +ColumnLimit: 120 +ConstructorInitializerAllOnOneLineOrOnePerLine: true +DerivePointerBinding: false +PointerBindsToType: true +ExperimentalAutoDetectBinPacking: false +IndentCaseLabels: true +MaxEmptyLinesToKeep: 1 +NamespaceIndentation: None +ObjCSpaceBeforeProtocolList: true +PenaltyBreakBeforeFirstCallParameter: 19 +PenaltyBreakComment: 60 +PenaltyBreakString: 1 +PenaltyBreakFirstLessLess: 1000 +PenaltyExcessCharacter: 1000 +PenaltyReturnTypeOnItsOwnLine: 90 +SpacesBeforeTrailingComments: 2 +Cpp11BracedListStyle: false +Standard: Auto +IndentWidth: 2 +TabWidth: 2 +UseTab: Never +IndentFunctionDeclarationAfterType: false +SpacesInParentheses: false +SpacesInAngles: false +SpaceInEmptyParentheses: false +SpacesInCStyleCastParentheses: false +SpaceAfterControlStatementKeyword: true +SpaceBeforeAssignmentOperators: true +ContinuationIndentWidth: 4 +SortIncludes: false +SpaceAfterCStyleCast: false + +# Configure each individual brace in BraceWrapping +BreakBeforeBraces: Custom + +# Control of individual brace wrapping cases +BraceWrapping: { + AfterClass: 'true' + AfterControlStatement: 'true' + AfterEnum : 'true' + AfterFunction : 'true' + AfterNamespace : 'true' + AfterStruct : 'true' + AfterUnion : 'true' + BeforeCatch : 'true' + BeforeElse : 'true' + IndentBraces : 'false' +} diff --git a/capabilities2_runner_prompt/CMakeLists.txt b/capabilities2_runner_prompt/CMakeLists.txt new file mode 100644 index 0000000..218496c --- /dev/null +++ b/capabilities2_runner_prompt/CMakeLists.txt @@ -0,0 +1,71 @@ +cmake_minimum_required(VERSION 3.8) +project(capabilities2_runner_prompt) + +# Default to C++17 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_action REQUIRED) +find_package(pluginlib REQUIRED) +find_package(prompt_msgs REQUIRED) +find_package(capabilities2_msgs REQUIRED) +find_package(capabilities2_runner REQUIRED) +find_package(PkgConfig) +pkg_check_modules(TINYXML2 tinyxml2 REQUIRED) + +include_directories(include +${TINYXML2_INCLUDE_DIRS} +) + +add_library(${PROJECT_NAME} SHARED + src/prompt_runners.cpp +) + +target_link_libraries(${PROJECT_NAME} + ${TINYXML2_LIBRARIES} +) + +ament_target_dependencies(${PROJECT_NAME} + rclcpp + rclcpp_action + pluginlib + prompt_msgs + capabilities2_msgs + capabilities2_runner + TINYXML2 +) + +pluginlib_export_plugin_description_file(${PROJECT_NAME} plugins.xml) + +install(TARGETS ${PROJECT_NAME} + EXPORT ${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +install(DIRECTORY include/ + DESTINATION include +) + +# install interface files +install(DIRECTORY interfaces + DESTINATION share/${PROJECT_NAME} +) + +# install providers files +install(DIRECTORY providers + DESTINATION share/${PROJECT_NAME} +) + +ament_export_include_directories(include) +ament_export_libraries(${PROJECT_NAME}) + +ament_package() diff --git a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_runner.hpp b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_runner.hpp new file mode 100644 index 0000000..61e9d2c --- /dev/null +++ b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_runner.hpp @@ -0,0 +1,94 @@ +#pragma once +#include +#include +#include +#include +#include +#include + +namespace capabilities2_runner +{ +/** + * @brief prompt capability runner + * + * This class is a wrapper around the capabilities2 service runner that is used to + * call on the prompt_tools/prompt service, providing it as a capability + */ +class PromptRunner : public ServiceRunner +{ +public: + PromptRunner() : ServiceRunner() + { + } + + /** + * @brief Starter function for starting the service runner + * + * @param node shared pointer to the capabilities node. Allows to use ros node related functionalities + * @param run_config runner configuration loaded from the yaml file + */ + virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config) override + { + init_service(node, run_config, "prompt"); + } + + /** + * @brief Generate a request from parameters given. + * + * This function is used in conjunction with the trigger function to inject type erased parameters + * into the typed action + * + * A pattern needs to be implemented in the derived class + * + * @param parameters + * @return prompt_msgs::srv::Prompt::Request the generated request + */ + virtual typename prompt_msgs::srv::Prompt::Request generate_request(tinyxml2::XMLElement* parameters) override + { + parameters_ = parameters; + + const char** text; + + parameters_->QueryStringAttribute("text", text); + + std::string text_data(*text); + + prompt_msgs::srv::Prompt::Request request; + + request.prompt.prompt = "The person responded with " + text_data; + + prompt_msgs::msg::ModelOption modelOption1; + modelOption1.key = "model"; + modelOption1.value = "llama3.1:8b"; + + request.prompt.options.push_back(modelOption1); + + prompt_msgs::msg::ModelOption modelOption2; + modelOption2.key = "stream"; + modelOption2.value = "false"; + modelOption2.type = prompt_msgs::msg::ModelOption::BOOL_TYPE; + + request.prompt.options.push_back(modelOption2); + + return request; + } + + /** + * @brief generate a typed erased response + * + * this method is used in a callback passed to the trigger caller to get type erased result + * from the service the reponse can be passed by the caller or ignored + * + * The pattern needs to be implemented in the derived class + * + * @param wrapped_result + * @return tinyxml2::XMLElement* + */ + virtual tinyxml2::XMLElement* + generate_response(const typename prompt_msgs::srv::Prompt::Response::SharedPtr& result) const override + { + return nullptr; + } +}; + +} // namespace capabilities2_runner diff --git a/capabilities2_runner_prompt/interfaces/prompt.yaml b/capabilities2_runner_prompt/interfaces/prompt.yaml new file mode 100644 index 0000000..0cfe8b3 --- /dev/null +++ b/capabilities2_runner_prompt/interfaces/prompt.yaml @@ -0,0 +1,19 @@ +%YAML 1.1 +--- +name: Prompt +spec_version: 1 +spec_type: interface +description: "This interface describes the basic ROS interface for interacting with the Prompt Bridge." +interface: + topics: + "/history": + type: "prompt_msgs/msg/PromptHistory" + description: "The history of prompts sent to the Prompt Bridge using the service interface" + services: + "/prompt": + type: "prompt_msgs/srv/Prompt" + description: "The prompt to send to the Prompt Bridge. returns the generated response" + actions: + "/prompt": + type: "prompt_msgs/action/Prompt" + description: "The prompt goal that requires a response, the action interface publishes partial results. returns the generated response" diff --git a/capabilities2_runner_prompt/package.xml b/capabilities2_runner_prompt/package.xml new file mode 100644 index 0000000..c16bc40 --- /dev/null +++ b/capabilities2_runner_prompt/package.xml @@ -0,0 +1,38 @@ + + + + capabilities2_runner_prompt + 0.0.0 + TODO: Package description + kalana + TODO: License declaration + + ament_cmake + + rclcpp + rclcpp_action + pluginlib + prompt_msgs + capabilities2_msgs + capabilities2_runner + + + tinyxml2 + + ament_lint_auto + ament_lint_common + + + ament_cmake + + + + interfaces/prompt.yaml + + + + + providers/PromptRunner.yaml + + + diff --git a/capabilities2_runner_prompt/plugins.xml b/capabilities2_runner_prompt/plugins.xml new file mode 100644 index 0000000..ade21e2 --- /dev/null +++ b/capabilities2_runner_prompt/plugins.xml @@ -0,0 +1,7 @@ + + + + A plugin that provide prompting capability + + + diff --git a/capabilities2_runner_prompt/providers/PromptRunner.yaml b/capabilities2_runner_prompt/providers/PromptRunner.yaml new file mode 100644 index 0000000..dc66838 --- /dev/null +++ b/capabilities2_runner_prompt/providers/PromptRunner.yaml @@ -0,0 +1,9 @@ +# the empty provider +%YAML 1.1 +--- +name: PromptRunner +spec_type: provider +spec_version: 1.1 +description: The capability provider for the prompting interface from prompt tools +implements: capabilities2_runner_prompt/PromptRunner +runner: capabilities2_runner::PromptRunner diff --git a/capabilities2_runner_prompt/src/prompt_runners.cpp b/capabilities2_runner_prompt/src/prompt_runners.cpp new file mode 100644 index 0000000..01973c8 --- /dev/null +++ b/capabilities2_runner_prompt/src/prompt_runners.cpp @@ -0,0 +1,11 @@ +#include +#include +#include + +namespace capabilities2_runner +{ + +} + +// register runner plugins +PLUGINLIB_EXPORT_CLASS(capabilities2_runner::PromptRunner, capabilities2_runner::RunnerBase) \ No newline at end of file From b253767a0bda3998a339d9991b1fc8041a23150f Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Tue, 29 Oct 2024 01:39:30 +1100 Subject: [PATCH 010/133] added eleborated yaml and tinyxml snippetes --- .../capabilities2_runner/action_runner.hpp | 4 + .../listen_runner.hpp | 37 ++- .../interfaces/speech_to_text.yaml | 10 +- .../occupancygrid_runner.hpp | 230 +++++++++++++++--- .../robotpose_runner.hpp | 103 ++++++-- capabilities2_runner_nav2/interfaces/map.yaml | 4 +- .../interfaces/pose.yaml | 4 +- .../prompt_occupancy_runner.hpp | 97 ++++++++ .../prompt_pose_runner.hpp | 97 ++++++++ ...ompt_runner.hpp => prompt_text_runner.hpp} | 20 +- capabilities2_runner_prompt/package.xml | 15 +- capabilities2_runner_prompt/plugins.xml | 14 +- .../providers/PromptOccupancyRunner.yaml | 9 + .../providers/PromptPoseRunner.yaml | 9 + .../providers/PromptRunner.yaml | 9 - .../providers/PromptTextRunner.yaml | 9 + .../semantic_interfaces.yaml | 14 ++ .../src/prompt_runners.cpp | 8 +- 18 files changed, 598 insertions(+), 95 deletions(-) create mode 100644 capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_occupancy_runner.hpp create mode 100644 capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_pose_runner.hpp rename capabilities2_runner_prompt/include/capabilities2_runner_prompt/{prompt_runner.hpp => prompt_text_runner.hpp} (83%) create mode 100644 capabilities2_runner_prompt/providers/PromptOccupancyRunner.yaml create mode 100644 capabilities2_runner_prompt/providers/PromptPoseRunner.yaml delete mode 100644 capabilities2_runner_prompt/providers/PromptRunner.yaml create mode 100644 capabilities2_runner_prompt/providers/PromptTextRunner.yaml create mode 100644 capabilities2_runner_prompt/semantic_interfaces/semantic_interfaces.yaml diff --git a/capabilities2_runner/include/capabilities2_runner/action_runner.hpp b/capabilities2_runner/include/capabilities2_runner/action_runner.hpp index f738617..f7d8197 100644 --- a/capabilities2_runner/include/capabilities2_runner/action_runner.hpp +++ b/capabilities2_runner/include/capabilities2_runner/action_runner.hpp @@ -148,6 +148,7 @@ class ActionRunner : public RunnerBase // send success event if (events[execute_id].on_success) { + result_ = wrapped_result.result; events[execute_id].on_success(update_on_success(events[execute_id].on_success_param)); execute_id += 1; } @@ -244,6 +245,9 @@ class ActionRunner : public RunnerBase /** goal handle parameter to capture goal response from goal_response_callback */ typename rclcpp_action::ClientGoalHandle::SharedPtr goal_handle_; + + /** Result */ + typename ActionT::Result::SharedPtr result_; }; } // namespace capabilities2_runner 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 53f4693..f507939 100644 --- a/capabilities2_runner_audio/include/capabilities2_runner_audio/listen_runner.hpp +++ b/capabilities2_runner_audio/include/capabilities2_runner_audio/listen_runner.hpp @@ -39,8 +39,9 @@ class ListenerRunner : public ActionRunner protected: /** * @brief This generate goal function overrides the generate_goal() function from ActionRunner() - * - * @param parameters XMLElement that contains parameters in the format '' + * + * @param parameters XMLElement that contains parameters in the format '' * @return ActionT::Goal the generated goal */ virtual hri_audio_msgs::action::SpeechToText::Goal generate_goal(tinyxml2::XMLElement* parameters) override @@ -56,20 +57,44 @@ class ListenerRunner : public ActionRunner /** * @brief This generate_result function overrides the generate_result() function from ActionRunner(). Since - * + * * @param result message from SpeechToText action * @return tinyxml2::XMLElement* in the format '' */ virtual tinyxml2::XMLElement* generate_result(const hri_audio_msgs::action::SpeechToText::Result::SharedPtr& result) override { - std::string text = result->stt_text; + tinyxml2::XMLDocument doc; - parameters_->SetAttribute( "text", text.c_str() ); + // Root element + tinyxml2::XMLElement* textElement = doc.NewElement("Text"); + doc.InsertEndChild(textElement); + textElement->SetText(result->stt_text.c_str()); - return parameters_; + return doc.FirstChildElement("Text"); } + /** + * @brief Update on_success event parameters with new data if avaible. + * + * This function is used to inject new data into the XMLElement containing + * parameters related to the on_success trigger event + * + * A pattern needs to be implemented in the derived class + * + * @param parameters pointer to the XMLElement containing parameters + * @return pointer to the XMLElement containing updated parameters + */ + virtual tinyxml2::XMLElement* update_on_success(tinyxml2::XMLElement* parameters) + { + // Create the Pose element as a child of the existing parameters element + tinyxml2::XMLElement* textElement = parameters->GetDocument()->NewElement("Text"); + parameters->InsertEndChild(textElement); + textElement->SetText(result_->stt_text.c_str()); + + // Return the updated parameters element with Pose added + return parameters; + }; }; } // namespace capabilities2_runner diff --git a/capabilities2_runner_audio/interfaces/speech_to_text.yaml b/capabilities2_runner_audio/interfaces/speech_to_text.yaml index 5da3a8f..b2d48b9 100644 --- a/capabilities2_runner_audio/interfaces/speech_to_text.yaml +++ b/capabilities2_runner_audio/interfaces/speech_to_text.yaml @@ -9,7 +9,9 @@ interface: actions: "speech_to_text": type: "hri_audio_msgs::action::SpeechToText" - description: "This system allow the robot to listen to sounds in the environment and then interpret that in the text format. The system can - triggered via '' command. After listening it will emit a output value which - has the format of where '$value' represents what the robot heard - as a text. As an example ''' means that the robot heard 'hello'. \ No newline at end of file + description: "This system allow the robot to listen to sounds in the environment and then interpret that in the text format. This enables + the decision making authority to understand the surrounding sounds, discussions regarding or with the robot.The system can + triggered via '' command.This generally needs to be followed by with a + prompting tool event that allows for transfer of data to the decision making authority. This runner will populate the prompting + tool's execution parameters with the relavent data. As an example if this is followed by prompt_text runner provided by + PromptTextRunner provider, such as '' \ No newline at end of file diff --git a/capabilities2_runner_nav2/include/capabilities2_runner_nav2/occupancygrid_runner.hpp b/capabilities2_runner_nav2/include/capabilities2_runner_nav2/occupancygrid_runner.hpp index 95f4a9a..f5f0ef3 100644 --- a/capabilities2_runner_nav2/include/capabilities2_runner_nav2/occupancygrid_runner.hpp +++ b/capabilities2_runner_nav2/include/capabilities2_runner_nav2/occupancygrid_runner.hpp @@ -44,35 +44,112 @@ class OccupancyGridRunner : public TopicRunner virtual tinyxml2::XMLElement* generate_message(typename nav_msgs::msg::OccupancyGrid::SharedPtr& result) override { - tinyxml2::XMLDocument* document = new tinyxml2::XMLDocument(); - tinyxml2::XMLNode* robot_pose = document->InsertEndChild(document->NewElement("OccupancyGrid")); + tinyxml2::XMLDocument doc; - tinyxml2::XMLElement* pose_element = document->NewElement("Info"); + // Root element for OccupancyGrid + tinyxml2::XMLElement* occupancyGridElement = doc.NewElement("OccupancyGrid"); + doc.InsertFirstChild(occupancyGridElement); - pose_element->SetAttribute("resolution", result->info.resolution); - pose_element->SetAttribute("width", result->info.width); - pose_element->SetAttribute("height", result->info.height); - pose_element->SetAttribute("origin.position.x", result->info.origin.position.x); - pose_element->SetAttribute("origin.position.y", result->info.origin.position.y); - pose_element->SetAttribute("origin.position.z", result->info.origin.position.z); - pose_element->SetAttribute("origin.orientation.x", result->info.origin.orientation.x); - pose_element->SetAttribute("origin.orientation.y", result->info.origin.orientation.y); - pose_element->SetAttribute("origin.orientation.z", result->info.origin.orientation.z); - pose_element->SetAttribute("origin.orientation.x", result->info.origin.orientation.w); + // Header element + tinyxml2::XMLElement* headerElement = doc.NewElement("header"); + occupancyGridElement->InsertEndChild(headerElement); - tinyxml2::XMLElement* data_element = document->NewElement("Data"); + // Header fields: stamp, frame_id + tinyxml2::XMLElement* stampElement = doc.NewElement("stamp"); + headerElement->InsertEndChild(stampElement); - std::string data_text = ""; + tinyxml2::XMLElement* secsElement = doc.NewElement("secs"); + secsElement->SetText(result->header.stamp.sec); + stampElement->InsertEndChild(secsElement); - for (const auto& data : result->data) - data_text += std::to_string(data) + " "; + tinyxml2::XMLElement* nsecsElement = doc.NewElement("nsecs"); + nsecsElement->SetText(result->header.stamp.nanosec); + stampElement->InsertEndChild(nsecsElement); - data_element->SetAttribute("data", data_text.c_str()); + tinyxml2::XMLElement* frameIdElement = doc.NewElement("frame_id"); + frameIdElement->SetText(result->header.frame_id.c_str()); + headerElement->InsertEndChild(frameIdElement); - robot_pose->InsertFirstChild(pose_element); - robot_pose->InsertEndChild(data_element); + // Info element + tinyxml2::XMLElement* infoElement = doc.NewElement("info"); + occupancyGridElement->InsertEndChild(infoElement); - return document->FirstChildElement("OccupancyGrid"); + // Map info fields: resolution, width, height + tinyxml2::XMLElement* resolutionElement = doc.NewElement("resolution"); + resolutionElement->SetText(result->info.resolution); + infoElement->InsertEndChild(resolutionElement); + + tinyxml2::XMLElement* widthElement = doc.NewElement("width"); + widthElement->SetText(result->info.width); + infoElement->InsertEndChild(widthElement); + + tinyxml2::XMLElement* heightElement = doc.NewElement("height"); + heightElement->SetText(result->info.height); + infoElement->InsertEndChild(heightElement); + + // Origin position element + tinyxml2::XMLElement* originElement = doc.NewElement("origin"); + infoElement->InsertEndChild(originElement); + + tinyxml2::XMLElement* positionElement = doc.NewElement("position"); + originElement->InsertEndChild(positionElement); + + tinyxml2::XMLElement* posX = doc.NewElement("x"); + posX->SetText(result->info.origin.position.x); + positionElement->InsertEndChild(posX); + + tinyxml2::XMLElement* posY = doc.NewElement("y"); + posY->SetText(result->info.origin.position.y); + positionElement->InsertEndChild(posY); + + tinyxml2::XMLElement* posZ = doc.NewElement("z"); + posZ->SetText(result->info.origin.position.z); + positionElement->InsertEndChild(posZ); + + // Origin orientation element + tinyxml2::XMLElement* orientationElement = doc.NewElement("orientation"); + originElement->InsertEndChild(orientationElement); + + tinyxml2::XMLElement* oriX = doc.NewElement("x"); + oriX->SetText(result->info.origin.orientation.x); + orientationElement->InsertEndChild(oriX); + + tinyxml2::XMLElement* oriY = doc.NewElement("y"); + oriY->SetText(result->info.origin.orientation.y); + orientationElement->InsertEndChild(oriY); + + tinyxml2::XMLElement* oriZ = doc.NewElement("z"); + oriZ->SetText(result->info.origin.orientation.z); + orientationElement->InsertEndChild(oriZ); + + tinyxml2::XMLElement* oriW = doc.NewElement("w"); + oriW->SetText(result->info.origin.orientation.w); + orientationElement->InsertEndChild(oriW); + + // Map load time + tinyxml2::XMLElement* mapLoadTimeElement = doc.NewElement("map_load_time"); + infoElement->InsertEndChild(mapLoadTimeElement); + + tinyxml2::XMLElement* mapSecsElement = doc.NewElement("secs"); + mapSecsElement->SetText(result->info.map_load_time.sec); + mapLoadTimeElement->InsertEndChild(mapSecsElement); + + tinyxml2::XMLElement* mapNsecsElement = doc.NewElement("nsecs"); + mapNsecsElement->SetText(result->info.map_load_time.nanosec); + mapLoadTimeElement->InsertEndChild(mapNsecsElement); + + // Data element for occupancy grid data + tinyxml2::XMLElement* dataElement = doc.NewElement("data"); + occupancyGridElement->InsertEndChild(dataElement); + + // Convert the occupancy grid data into a space-separated string + std::string data_str; + for (const auto& value : result->data) { + data_str += std::to_string(value) + " "; + } + dataElement->SetText(data_str.c_str()); + + return doc.FirstChildElement("OccupancyGrid"); } /** @@ -88,25 +165,106 @@ class OccupancyGridRunner : public TopicRunner */ virtual tinyxml2::XMLElement* update_on_success(tinyxml2::XMLElement* parameters) { - std::string data_text = ""; + // Create the OccupancyGrid element as a child of the existing parameters element + tinyxml2::XMLElement* occupancyGridElement = parameters->GetDocument()->NewElement("OccupancyGrid"); + parameters->InsertEndChild(occupancyGridElement); + + // Header element + tinyxml2::XMLElement* headerElement = parameters->GetDocument()->NewElement("header"); + occupancyGridElement->InsertEndChild(headerElement); + + tinyxml2::XMLElement* stampElement = parameters->GetDocument()->NewElement("stamp"); + headerElement->InsertEndChild(stampElement); + + tinyxml2::XMLElement* secsElement = parameters->GetDocument()->NewElement("secs"); + secsElement->SetText(latest_message_->header.stamp.sec); + stampElement->InsertEndChild(secsElement); + + tinyxml2::XMLElement* nsecsElement = parameters->GetDocument()->NewElement("nsecs"); + nsecsElement->SetText(latest_message_->header.stamp.nanosec); + stampElement->InsertEndChild(nsecsElement); + + tinyxml2::XMLElement* frameIdElement = parameters->GetDocument()->NewElement("frame_id"); + frameIdElement->SetText(latest_message_->header.frame_id.c_str()); + headerElement->InsertEndChild(frameIdElement); + + // Info element + tinyxml2::XMLElement* infoElement = parameters->GetDocument()->NewElement("info"); + occupancyGridElement->InsertEndChild(infoElement); + + tinyxml2::XMLElement* resolutionElement = parameters->GetDocument()->NewElement("resolution"); + resolutionElement->SetText(latest_message_->info.resolution); + infoElement->InsertEndChild(resolutionElement); + + tinyxml2::XMLElement* widthElement = parameters->GetDocument()->NewElement("width"); + widthElement->SetText(latest_message_->info.width); + infoElement->InsertEndChild(widthElement); + + tinyxml2::XMLElement* heightElement = parameters->GetDocument()->NewElement("height"); + heightElement->SetText(latest_message_->info.height); + infoElement->InsertEndChild(heightElement); + + // Origin element + tinyxml2::XMLElement* originElement = parameters->GetDocument()->NewElement("origin"); + infoElement->InsertEndChild(originElement); + + tinyxml2::XMLElement* positionElement = parameters->GetDocument()->NewElement("position"); + originElement->InsertEndChild(positionElement); + + tinyxml2::XMLElement* posX = parameters->GetDocument()->NewElement("x"); + posX->SetText(latest_message_->info.origin.position.x); + positionElement->InsertEndChild(posX); + + tinyxml2::XMLElement* posY = parameters->GetDocument()->NewElement("y"); + posY->SetText(latest_message_->info.origin.position.y); + positionElement->InsertEndChild(posY); + + tinyxml2::XMLElement* posZ = parameters->GetDocument()->NewElement("z"); + posZ->SetText(latest_message_->info.origin.position.z); + positionElement->InsertEndChild(posZ); + + tinyxml2::XMLElement* orientationElement = parameters->GetDocument()->NewElement("orientation"); + originElement->InsertEndChild(orientationElement); + + tinyxml2::XMLElement* oriX = parameters->GetDocument()->NewElement("x"); + oriX->SetText(latest_message_->info.origin.orientation.x); + orientationElement->InsertEndChild(oriX); + + tinyxml2::XMLElement* oriY = parameters->GetDocument()->NewElement("y"); + oriY->SetText(latest_message_->info.origin.orientation.y); + orientationElement->InsertEndChild(oriY); + + tinyxml2::XMLElement* oriZ = parameters->GetDocument()->NewElement("z"); + oriZ->SetText(latest_message_->info.origin.orientation.z); + orientationElement->InsertEndChild(oriZ); + + tinyxml2::XMLElement* oriW = parameters->GetDocument()->NewElement("w"); + oriW->SetText(latest_message_->info.origin.orientation.w); + orientationElement->InsertEndChild(oriW); + + // Map load time + tinyxml2::XMLElement* mapLoadTimeElement = parameters->GetDocument()->NewElement("map_load_time"); + infoElement->InsertEndChild(mapLoadTimeElement); + + tinyxml2::XMLElement* mapSecsElement = parameters->GetDocument()->NewElement("secs"); + mapSecsElement->SetText(latest_message_->info.map_load_time.sec); + mapLoadTimeElement->InsertEndChild(mapSecsElement); - for (const auto& data : latest_message_->data) - data_text += std::to_string(data) + " "; + tinyxml2::XMLElement* mapNsecsElement = parameters->GetDocument()->NewElement("nsecs"); + mapNsecsElement->SetText(latest_message_->info.map_load_time.nanosec); + mapLoadTimeElement->InsertEndChild(mapNsecsElement); - parameters->SetAttribute("type", "nav_msgs/msgs/OccupancyGrid"); + // Data element for occupancy data (converted to a string) + tinyxml2::XMLElement* dataElement = parameters->GetDocument()->NewElement("data"); + occupancyGridElement->InsertEndChild(dataElement); - parameters->SetAttribute("info.resolution", latest_message_->info.resolution); - parameters->SetAttribute("info.width", latest_message_->info.width); - parameters->SetAttribute("info.height", latest_message_->info.height); - parameters->SetAttribute("info.origin.position.x", latest_message_->info.origin.position.x); - parameters->SetAttribute("info.origin.position.y", latest_message_->info.origin.position.y); - parameters->SetAttribute("info.origin.position.z", latest_message_->info.origin.position.z); - parameters->SetAttribute("info.origin.orientation.x", latest_message_->info.origin.orientation.x); - parameters->SetAttribute("info.origin.orientation.y", latest_message_->info.origin.orientation.y); - parameters->SetAttribute("info.origin.orientation.z", latest_message_->info.origin.orientation.z); - parameters->SetAttribute("info.origin.orientation.x", latest_message_->info.origin.orientation.w); - parameters->SetAttribute("data", data_text.c_str()); + std::string data_str; + for (size_t i = 0; i < latest_message_->data.size(); ++i) { + data_str += std::to_string(latest_message_->data[i]) + " "; + } + dataElement->SetText(data_str.c_str()); + // Return the updated parameters element with OccupancyGrid added return parameters; }; }; diff --git a/capabilities2_runner_nav2/include/capabilities2_runner_nav2/robotpose_runner.hpp b/capabilities2_runner_nav2/include/capabilities2_runner_nav2/robotpose_runner.hpp index ac5fe66..83904d7 100644 --- a/capabilities2_runner_nav2/include/capabilities2_runner_nav2/robotpose_runner.hpp +++ b/capabilities2_runner_nav2/include/capabilities2_runner_nav2/robotpose_runner.hpp @@ -44,26 +44,51 @@ class RobotPoseRunner : public TopicRunnerInsertEndChild(document->NewElement("RobotPose")); + tinyxml2::XMLDocument doc; - tinyxml2::XMLElement* pose_element = document->NewElement("Pose"); + // Root element + tinyxml2::XMLElement* poseElement = doc.NewElement("Pose"); + doc.InsertFirstChild(poseElement); - pose_element->SetAttribute("x", result->pose.pose.position.x); - pose_element->SetAttribute("y", result->pose.pose.position.y); - pose_element->SetAttribute("z", result->pose.pose.position.z); + // Position element + tinyxml2::XMLElement* positionElement = doc.NewElement("position"); + poseElement->InsertEndChild(positionElement); - tinyxml2::XMLElement* orie_element = document->NewElement("Orientation"); + // Position x, y, z + tinyxml2::XMLElement* posX = doc.NewElement("x"); + posX->SetText(result->pose.pose.position.x); // Set x position value + positionElement->InsertEndChild(posX); - orie_element->SetAttribute("x", result->pose.pose.orientation.x); - orie_element->SetAttribute("y", result->pose.pose.orientation.y); - orie_element->SetAttribute("z", result->pose.pose.orientation.z); - orie_element->SetAttribute("w", result->pose.pose.orientation.w); + tinyxml2::XMLElement* posY = doc.NewElement("y"); + posY->SetText(result->pose.pose.position.y); // Set y position value + positionElement->InsertEndChild(posY); - robot_pose->InsertFirstChild(pose_element); - robot_pose->InsertEndChild(orie_element); + tinyxml2::XMLElement* posZ = doc.NewElement("z"); + posZ->SetText(result->pose.pose.position.z); // Set z position value + positionElement->InsertEndChild(posZ); - return document->FirstChildElement("RobotPose"); + // Orientation element + tinyxml2::XMLElement* orientationElement = doc.NewElement("orientation"); + poseElement->InsertEndChild(orientationElement); + + // Orientation x, y, z, w + tinyxml2::XMLElement* oriX = doc.NewElement("x"); + oriX->SetText(result->pose.pose.orientation.x); // Set orientation x value + orientationElement->InsertEndChild(oriX); + + tinyxml2::XMLElement* oriY = doc.NewElement("y"); + oriY->SetText(result->pose.pose.orientation.y); // Set orientation y value + orientationElement->InsertEndChild(oriY); + + tinyxml2::XMLElement* oriZ = doc.NewElement("z"); + oriZ->SetText(result->pose.pose.orientation.z); // Set orientation z value + orientationElement->InsertEndChild(oriZ); + + tinyxml2::XMLElement* oriW = doc.NewElement("w"); + oriW->SetText(result->pose.pose.orientation.w); // Set orientation w value + orientationElement->InsertEndChild(oriW); + + return doc.FirstChildElement("Pose"); } /** @@ -79,15 +104,49 @@ class RobotPoseRunner : public TopicRunnerSetAttribute("type", "geometry_msgs/msgs/Pose"); - parameters->SetAttribute("position.x", latest_message_->pose.pose.position.x); - parameters->SetAttribute("position.y", latest_message_->pose.pose.position.y); - parameters->SetAttribute("position.z", latest_message_->pose.pose.position.z); - parameters->SetAttribute("orientation.x", latest_message_->pose.pose.orientation.x); - parameters->SetAttribute("orientation.y", latest_message_->pose.pose.orientation.y); - parameters->SetAttribute("orientation.z", latest_message_->pose.pose.orientation.z); - parameters->SetAttribute("orientation.x", latest_message_->pose.pose.orientation.w); + // Create the Pose element as a child of the existing parameters element + tinyxml2::XMLElement* poseElement = parameters->GetDocument()->NewElement("Pose"); + parameters->InsertEndChild(poseElement); + + // Position element + tinyxml2::XMLElement* positionElement = parameters->GetDocument()->NewElement("position"); + poseElement->InsertEndChild(positionElement); + + // Position x, y, z + tinyxml2::XMLElement* posX = parameters->GetDocument()->NewElement("x"); + posX->SetText(latest_message_->pose.pose.position.x); // Set x position value + positionElement->InsertEndChild(posX); + + tinyxml2::XMLElement* posY = parameters->GetDocument()->NewElement("y"); + posY->SetText(latest_message_->pose.pose.position.y); // Set y position value + positionElement->InsertEndChild(posY); + + tinyxml2::XMLElement* posZ = parameters->GetDocument()->NewElement("z"); + posZ->SetText(latest_message_->pose.pose.position.z); // Set z position value + positionElement->InsertEndChild(posZ); + + // Orientation element + tinyxml2::XMLElement* orientationElement = parameters->GetDocument()->NewElement("orientation"); + poseElement->InsertEndChild(orientationElement); + + // Orientation x, y, z, w + tinyxml2::XMLElement* oriX = parameters->GetDocument()->NewElement("x"); + oriX->SetText(latest_message_->pose.pose.orientation.x); // Set orientation x value + orientationElement->InsertEndChild(oriX); + + tinyxml2::XMLElement* oriY = parameters->GetDocument()->NewElement("y"); + oriY->SetText(latest_message_->pose.pose.orientation.y); // Set orientation y value + orientationElement->InsertEndChild(oriY); + + tinyxml2::XMLElement* oriZ = parameters->GetDocument()->NewElement("z"); + oriZ->SetText(latest_message_->pose.pose.orientation.z); // Set orientation z value + orientationElement->InsertEndChild(oriZ); + + tinyxml2::XMLElement* oriW = parameters->GetDocument()->NewElement("w"); + oriW->SetText(latest_message_->pose.pose.orientation.w); // Set orientation w value + orientationElement->InsertEndChild(oriW); + // Return the updated parameters element with Pose added return parameters; }; }; diff --git a/capabilities2_runner_nav2/interfaces/map.yaml b/capabilities2_runner_nav2/interfaces/map.yaml index e367948..030362b 100644 --- a/capabilities2_runner_nav2/interfaces/map.yaml +++ b/capabilities2_runner_nav2/interfaces/map.yaml @@ -13,5 +13,5 @@ interface: which enables the decision making authority to understand the environment around the robot. This can be triggered with a '' command. This generally needs to be followed by with a prompting tool event that allows for transfer of data to the decision making authority. This runner will populate the prompting tool's - execution parameters with the relavent data. As an example if this is followed by prompt_message runner provided by - PromptToolsRunner provider, such as ''. \ No newline at end of file + execution parameters with the relavent data. As an example if this is followed by prompt_occupancy runner provided by + PromptOccupancyRunner provider, such as ''. \ No newline at end of file diff --git a/capabilities2_runner_nav2/interfaces/pose.yaml b/capabilities2_runner_nav2/interfaces/pose.yaml index 02cd7d7..44b7724 100644 --- a/capabilities2_runner_nav2/interfaces/pose.yaml +++ b/capabilities2_runner_nav2/interfaces/pose.yaml @@ -13,5 +13,5 @@ interface: the decision making authority to understand the position and orientation of the robot. This can be triggered with a '' command. This generally needs to be followed by with a prompting tool event that allows for transfer of data to the decision making authority. This runner will populate the prompting tool's - execution parameters with the relavent data. As an example if this is followed by prompt_message runner provided by - PromptToolsRunner provider, such as '' \ No newline at end of file + execution parameters with the relavent data. As an example if this is followed by prompt_pose runner provided by + PromptPoseRunner provider, such as '' \ No newline at end of file diff --git a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_occupancy_runner.hpp b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_occupancy_runner.hpp new file mode 100644 index 0000000..d6782e4 --- /dev/null +++ b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_occupancy_runner.hpp @@ -0,0 +1,97 @@ +#pragma once +#include +#include +#include +#include +#include +#include + +namespace capabilities2_runner +{ +/** + * @brief prompt capability runner + * + * This class is a wrapper around the capabilities2 service runner and is used to + * call on the prompt_tools/prompt service, providing it as a capability that prompts + * text values + */ +class PromptOccupancyRunner : public ServiceRunner +{ +public: + PromptOccupancyRunner() : ServiceRunner() + { + } + + /** + * @brief Starter function for starting the service runner + * + * @param node shared pointer to the capabilities node. Allows to use ros node related functionalities + * @param run_config runner configuration loaded from the yaml file + */ + virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config) override + { + init_service(node, run_config, "prompt_occupancy"); + } + + /** + * @brief Generate a request from parameters given. + * + * This function is used in conjunction with the trigger function to inject type erased parameters + * into the typed action + * + * A pattern needs to be implemented in the derived class + * + * @param parameters + * @return prompt_msgs::srv::Prompt::Request the generated request + */ + virtual typename prompt_msgs::srv::Prompt::Request generate_request(tinyxml2::XMLElement* parameters) override + { + parameters_ = parameters; + + tinyxml2::XMLElement* occupancyElement = parameters->FirstChildElement("OccupancyGrid"); + + tinyxml2::XMLPrinter printer; + occupancyElement->Accept(&printer); + + std::string data(printer.CStr()); + + prompt_msgs::srv::Prompt::Request request; + + request.prompt.prompt = + "The OccupancyGrid of the robot is given as a ros2 nav_msgs/occupancy_grid of which the content are " + data; + + prompt_msgs::msg::ModelOption modelOption1; + modelOption1.key = "model"; + modelOption1.value = "llama3.1:8b"; + + request.prompt.options.push_back(modelOption1); + + prompt_msgs::msg::ModelOption modelOption2; + modelOption2.key = "stream"; + modelOption2.value = "false"; + modelOption2.type = prompt_msgs::msg::ModelOption::BOOL_TYPE; + + request.prompt.options.push_back(modelOption2); + + return request; + } + + /** + * @brief generate a typed erased response + * + * this method is used in a callback passed to the trigger caller to get type erased result + * from the service the reponse can be passed by the caller or ignored + * + * The pattern needs to be implemented in the derived class + * + * @param wrapped_result + * @return tinyxml2::XMLElement* + */ + virtual tinyxml2::XMLElement* + generate_response(const typename prompt_msgs::srv::Prompt::Response::SharedPtr& result) const override + { + return nullptr; + } +}; + +} // namespace capabilities2_runner diff --git a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_pose_runner.hpp b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_pose_runner.hpp new file mode 100644 index 0000000..9556888 --- /dev/null +++ b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_pose_runner.hpp @@ -0,0 +1,97 @@ +#pragma once +#include +#include +#include +#include +#include +#include + +namespace capabilities2_runner +{ +/** + * @brief prompt capability runner + * + * This class is a wrapper around the capabilities2 service runner and is used to + * call on the prompt_tools/prompt service, providing it as a capability that prompts + * text values + */ +class PromptPoseRunner : public ServiceRunner +{ +public: + PromptPoseRunner() : ServiceRunner() + { + } + + /** + * @brief Starter function for starting the service runner + * + * @param node shared pointer to the capabilities node. Allows to use ros node related functionalities + * @param run_config runner configuration loaded from the yaml file + */ + virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config) override + { + init_service(node, run_config, "prompt_pose"); + } + + /** + * @brief Generate a request from parameters given. + * + * This function is used in conjunction with the trigger function to inject type erased parameters + * into the typed action + * + * A pattern needs to be implemented in the derived class + * + * @param parameters + * @return prompt_msgs::srv::Prompt::Request the generated request + */ + virtual typename prompt_msgs::srv::Prompt::Request generate_request(tinyxml2::XMLElement* parameters) override + { + parameters_ = parameters; + + tinyxml2::XMLElement* poseElement = parameters->FirstChildElement("Pose"); + + tinyxml2::XMLPrinter printer; + poseElement->Accept(&printer); + + std::string data(printer.CStr()); + + prompt_msgs::srv::Prompt::Request request; + + request.prompt.prompt = + "The position of the robot is given as a standard ros2 geometry message of which the content are " + data; + + prompt_msgs::msg::ModelOption modelOption1; + modelOption1.key = "model"; + modelOption1.value = "llama3.1:8b"; + + request.prompt.options.push_back(modelOption1); + + prompt_msgs::msg::ModelOption modelOption2; + modelOption2.key = "stream"; + modelOption2.value = "false"; + modelOption2.type = prompt_msgs::msg::ModelOption::BOOL_TYPE; + + request.prompt.options.push_back(modelOption2); + + return request; + } + + /** + * @brief generate a typed erased response + * + * this method is used in a callback passed to the trigger caller to get type erased result + * from the service the reponse can be passed by the caller or ignored + * + * The pattern needs to be implemented in the derived class + * + * @param wrapped_result + * @return tinyxml2::XMLElement* + */ + virtual tinyxml2::XMLElement* + generate_response(const typename prompt_msgs::srv::Prompt::Response::SharedPtr& result) const override + { + return nullptr; + } +}; + +} // namespace capabilities2_runner diff --git a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_runner.hpp b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_text_runner.hpp similarity index 83% rename from capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_runner.hpp rename to capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_text_runner.hpp index 61e9d2c..120c4bc 100644 --- a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_runner.hpp +++ b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_text_runner.hpp @@ -11,13 +11,14 @@ namespace capabilities2_runner /** * @brief prompt capability runner * - * This class is a wrapper around the capabilities2 service runner that is used to - * call on the prompt_tools/prompt service, providing it as a capability + * This class is a wrapper around the capabilities2 service runner and is used to + * call on the prompt_tools/prompt service, providing it as a capability that prompts + * text values */ -class PromptRunner : public ServiceRunner +class PromptTextRunner : public ServiceRunner { public: - PromptRunner() : ServiceRunner() + PromptTextRunner() : ServiceRunner() { } @@ -29,7 +30,7 @@ class PromptRunner : public ServiceRunner */ virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config) override { - init_service(node, run_config, "prompt"); + init_service(node, run_config, "prompt_text"); } /** @@ -47,15 +48,16 @@ class PromptRunner : public ServiceRunner { parameters_ = parameters; - const char** text; + tinyxml2::XMLElement* textElement = parameters->FirstChildElement("Text"); - parameters_->QueryStringAttribute("text", text); + tinyxml2::XMLPrinter printer; + textElement->Accept(&printer); - std::string text_data(*text); + std::string data(printer.CStr()); prompt_msgs::srv::Prompt::Request request; - request.prompt.prompt = "The person responded with " + text_data; + request.prompt.prompt = "The response was " + data; prompt_msgs::msg::ModelOption modelOption1; modelOption1.key = "model"; diff --git a/capabilities2_runner_prompt/package.xml b/capabilities2_runner_prompt/package.xml index c16bc40..162481e 100644 --- a/capabilities2_runner_prompt/package.xml +++ b/capabilities2_runner_prompt/package.xml @@ -30,9 +30,22 @@ interfaces/prompt.yaml + + + semantic_interfaces/semantic_interfaces.yaml + + - providers/PromptRunner.yaml + providers/PromptTextRunner.yaml + + + + providers/PromptPoseRunner.yaml + + + + providers/PromptOccupancyRunner.yaml diff --git a/capabilities2_runner_prompt/plugins.xml b/capabilities2_runner_prompt/plugins.xml index ade21e2..c5ee3ce 100644 --- a/capabilities2_runner_prompt/plugins.xml +++ b/capabilities2_runner_prompt/plugins.xml @@ -1,7 +1,17 @@ - + - A plugin that provide prompting capability + A plugin that provide text prompting capability + + + + + A plugin that provide pose prompting capability + + + + + A plugin that provide occupancy grid map prompting capability diff --git a/capabilities2_runner_prompt/providers/PromptOccupancyRunner.yaml b/capabilities2_runner_prompt/providers/PromptOccupancyRunner.yaml new file mode 100644 index 0000000..1c33b76 --- /dev/null +++ b/capabilities2_runner_prompt/providers/PromptOccupancyRunner.yaml @@ -0,0 +1,9 @@ +# the empty provider +%YAML 1.1 +--- +name: PromptOccupancyRunner +spec_type: provider +spec_version: 1.1 +description: The capability provider for the prompting interface from prompt tools. This can be used for prompting the occupancy map around the robot. +implements: capabilities2_runner_prompt/PromptOccupancyRunner +runner: capabilities2_runner::PromptOccupancyRunner diff --git a/capabilities2_runner_prompt/providers/PromptPoseRunner.yaml b/capabilities2_runner_prompt/providers/PromptPoseRunner.yaml new file mode 100644 index 0000000..11adfbe --- /dev/null +++ b/capabilities2_runner_prompt/providers/PromptPoseRunner.yaml @@ -0,0 +1,9 @@ +# the empty provider +%YAML 1.1 +--- +name: PromptPoseRunner +spec_type: provider +spec_version: 1.1 +description: The capability provider for the prompting interface from prompt tools. This can be used for prompting pose of the robot. +implements: capabilities2_runner_prompt/PromptPoseRunner +runner: capabilities2_runner::PromptPoseRunner diff --git a/capabilities2_runner_prompt/providers/PromptRunner.yaml b/capabilities2_runner_prompt/providers/PromptRunner.yaml deleted file mode 100644 index dc66838..0000000 --- a/capabilities2_runner_prompt/providers/PromptRunner.yaml +++ /dev/null @@ -1,9 +0,0 @@ -# the empty provider -%YAML 1.1 ---- -name: PromptRunner -spec_type: provider -spec_version: 1.1 -description: The capability provider for the prompting interface from prompt tools -implements: capabilities2_runner_prompt/PromptRunner -runner: capabilities2_runner::PromptRunner diff --git a/capabilities2_runner_prompt/providers/PromptTextRunner.yaml b/capabilities2_runner_prompt/providers/PromptTextRunner.yaml new file mode 100644 index 0000000..98171c8 --- /dev/null +++ b/capabilities2_runner_prompt/providers/PromptTextRunner.yaml @@ -0,0 +1,9 @@ +# the empty provider +%YAML 1.1 +--- +name: PromptTextRunner +spec_type: provider +spec_version: 1.1 +description: The capability provider for the prompting interface from prompt tools. This can be used for prompting text. +implements: capabilities2_runner_prompt/PromptTextRunner +runner: capabilities2_runner::PromptTextRunner diff --git a/capabilities2_runner_prompt/semantic_interfaces/semantic_interfaces.yaml b/capabilities2_runner_prompt/semantic_interfaces/semantic_interfaces.yaml new file mode 100644 index 0000000..e833931 --- /dev/null +++ b/capabilities2_runner_prompt/semantic_interfaces/semantic_interfaces.yaml @@ -0,0 +1,14 @@ +# redefine the empty interface +%YAML 1.1 +--- +name: prompt_semantic_interface +spec_type: semantic_interface +spec_version: 1.1 +description: semantic interface for prompt_tools related runners that allow for multiple impletation towards prompt service +redefines: capabilities2_runner_prompt/prompt +global_namespace: +remappings: + services: + "prompt": "prompt_text" + "prompt": "prompt_pose" + "prompt": "prompt_occupancy" diff --git a/capabilities2_runner_prompt/src/prompt_runners.cpp b/capabilities2_runner_prompt/src/prompt_runners.cpp index 01973c8..9bac97c 100644 --- a/capabilities2_runner_prompt/src/prompt_runners.cpp +++ b/capabilities2_runner_prompt/src/prompt_runners.cpp @@ -1,6 +1,8 @@ #include #include -#include +#include +#include +#include namespace capabilities2_runner { @@ -8,4 +10,6 @@ namespace capabilities2_runner } // register runner plugins -PLUGINLIB_EXPORT_CLASS(capabilities2_runner::PromptRunner, capabilities2_runner::RunnerBase) \ No newline at end of file +PLUGINLIB_EXPORT_CLASS(capabilities2_runner::PromptTextRunner, capabilities2_runner::RunnerBase) +PLUGINLIB_EXPORT_CLASS(capabilities2_runner::PromptPoseRunner, capabilities2_runner::RunnerBase) +PLUGINLIB_EXPORT_CLASS(capabilities2_runner::PromptOccupancyRunner, capabilities2_runner::RunnerBase) \ No newline at end of file From 3221221ca38fcb5ab05a268943bf27544c3caae6 Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Tue, 29 Oct 2024 12:49:27 +1100 Subject: [PATCH 011/133] minorfixes --- .../capabilities_launch_proxy.py | 3 +-- .../interfaces/speech_to_text.yaml | 9 ++++----- .../interfaces/text_to_speech.yaml | 9 ++++----- .../interfaces/follow_waypoints.yaml | 7 +++---- capabilities2_runner_nav2/interfaces/map.yaml | 9 ++++----- capabilities2_runner_nav2/interfaces/pose.yaml | 9 ++++----- capabilities2_runner_prompt/interfaces/prompt.yaml | 2 +- .../semantic_interfaces/semantic_interfaces.yaml | 6 +++--- capabilities2_server/config/capabilities.yaml | 3 ++- 9 files changed, 26 insertions(+), 31 deletions(-) diff --git a/capabilities2_launch_proxy/capabilities2_launch_proxy/capabilities_launch_proxy.py b/capabilities2_launch_proxy/capabilities2_launch_proxy/capabilities_launch_proxy.py index c9edf53..3b495df 100755 --- a/capabilities2_launch_proxy/capabilities2_launch_proxy/capabilities_launch_proxy.py +++ b/capabilities2_launch_proxy/capabilities2_launch_proxy/capabilities_launch_proxy.py @@ -30,7 +30,6 @@ from launch.actions import EmitEvent from launch.actions import RegisterEventHandler from launch.launch_description_sources import AnyLaunchDescriptionSource -from launch.some_entities_type import SomeEntitiesType from capabilities2_msgs.action import Launch from capabilities2_msgs.msg import CapabilityEvent @@ -67,7 +66,7 @@ def __init__(self, goal_handle: ServerGoalHandle, pids: Set[int]): self.goal_handle = goal_handle self.pids = pids - def handle(self, event: Event, context: LaunchContext) -> Optional[SomeEntitiesType]: + def handle(self, event: Event, context: LaunchContext): """ handle event """ diff --git a/capabilities2_runner_audio/interfaces/speech_to_text.yaml b/capabilities2_runner_audio/interfaces/speech_to_text.yaml index b2d48b9..53f5cde 100644 --- a/capabilities2_runner_audio/interfaces/speech_to_text.yaml +++ b/capabilities2_runner_audio/interfaces/speech_to_text.yaml @@ -1,17 +1,16 @@ -# standardised nav2 interface specification %YAML 1.1 --- -name: speech_to_text -spec_version: 1 +name: SpeechToText +spec_version: 1.1 spec_type: interface description: "Speech to text capabilities using Audio stack" interface: actions: - "speech_to_text": + "/speech_to_text": type: "hri_audio_msgs::action::SpeechToText" description: "This system allow the robot to listen to sounds in the environment and then interpret that in the text format. This enables the decision making authority to understand the surrounding sounds, discussions regarding or with the robot.The system can triggered via '' command.This generally needs to be followed by with a prompting tool event that allows for transfer of data to the decision making authority. This runner will populate the prompting tool's execution parameters with the relavent data. As an example if this is followed by prompt_text runner provided by - PromptTextRunner provider, such as '' \ No newline at end of file + PromptTextRunner provider, such as ''" \ No newline at end of file diff --git a/capabilities2_runner_audio/interfaces/text_to_speech.yaml b/capabilities2_runner_audio/interfaces/text_to_speech.yaml index d686e0a..f4ab34c 100644 --- a/capabilities2_runner_audio/interfaces/text_to_speech.yaml +++ b/capabilities2_runner_audio/interfaces/text_to_speech.yaml @@ -1,16 +1,15 @@ -# standardised nav2 interface specification %YAML 1.1 --- -name: text_to_speech -spec_version: 1 +name: TextToSpeech +spec_version: 1.1 spec_type: interface description: "Text to Speech capabilities using Audio stack" interface: actions: - "text_to_speech": + "/text_to_speech": type: "hri_audio_msgs::action::TextToSpeech" description: "This system allow the robot to speak a given text. The system can triggered via '' command. where '$value' represents what the robot has to speak. if the robot recieves an command '', the robot - will speak 'how are you'. \ No newline at end of file + will speak 'how are you'." \ No newline at end of file diff --git a/capabilities2_runner_nav2/interfaces/follow_waypoints.yaml b/capabilities2_runner_nav2/interfaces/follow_waypoints.yaml index befe9a3..4e7dcbb 100644 --- a/capabilities2_runner_nav2/interfaces/follow_waypoints.yaml +++ b/capabilities2_runner_nav2/interfaces/follow_waypoints.yaml @@ -1,13 +1,12 @@ -# standardised nav2 interface specification %YAML 1.1 --- -name: follow_waypoints -spec_version: 1 +name: FollowWaypoints +spec_version: 1.1 spec_type: interface description: "Waypoint follower capabilities using Nav2 stack" interface: actions: - "follow_waypoints": + "/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_runner_nav2/interfaces/map.yaml b/capabilities2_runner_nav2/interfaces/map.yaml index 030362b..dcfd9c3 100644 --- a/capabilities2_runner_nav2/interfaces/map.yaml +++ b/capabilities2_runner_nav2/interfaces/map.yaml @@ -1,17 +1,16 @@ -# standardised nav2 interface specification %YAML 1.1 --- -name: map -spec_version: 1 +name: Map +spec_version: 1.1 spec_type: interface description: "data extraction capability to extract occupancy grid map of the robot" interface: topics: - "map": + "/map": type: "nav_msgs::msg::OccupancyGrid" description: "This system allows the extraction of the occupancy grid map produced by the robot's mapping or localization system. which enables the decision making authority to understand the environment around the robot. This can be triggered with a '' command. This generally needs to be followed by with a prompting tool event that allows for transfer of data to the decision making authority. This runner will populate the prompting tool's execution parameters with the relavent data. As an example if this is followed by prompt_occupancy runner provided by - PromptOccupancyRunner provider, such as ''. \ No newline at end of file + PromptOccupancyRunner provider, such as ''." \ No newline at end of file diff --git a/capabilities2_runner_nav2/interfaces/pose.yaml b/capabilities2_runner_nav2/interfaces/pose.yaml index 44b7724..4fec08c 100644 --- a/capabilities2_runner_nav2/interfaces/pose.yaml +++ b/capabilities2_runner_nav2/interfaces/pose.yaml @@ -1,17 +1,16 @@ -# standardised nav2 interface specification %YAML 1.1 --- -name: pose -spec_version: 1 +name: Pose +spec_version: 1.1 spec_type: interface description: "data extraction capability to extract pose of the robot" interface: topics: - "pose": + "/pose": type: "geometry_msgs::msg::PoseWithCovarianceStamped" description: "This system allows the extraction of the pose of the robot from the odoemtry or localization system. which enables the decision making authority to understand the position and orientation of the robot. This can be triggered with a '' command. This generally needs to be followed by with a prompting tool event that allows for transfer of data to the decision making authority. This runner will populate the prompting tool's execution parameters with the relavent data. As an example if this is followed by prompt_pose runner provided by - PromptPoseRunner provider, such as '' \ No newline at end of file + PromptPoseRunner provider, such as ''" \ No newline at end of file diff --git a/capabilities2_runner_prompt/interfaces/prompt.yaml b/capabilities2_runner_prompt/interfaces/prompt.yaml index 0cfe8b3..a537362 100644 --- a/capabilities2_runner_prompt/interfaces/prompt.yaml +++ b/capabilities2_runner_prompt/interfaces/prompt.yaml @@ -1,7 +1,7 @@ %YAML 1.1 --- name: Prompt -spec_version: 1 +spec_version: 1.1 spec_type: interface description: "This interface describes the basic ROS interface for interacting with the Prompt Bridge." interface: diff --git a/capabilities2_runner_prompt/semantic_interfaces/semantic_interfaces.yaml b/capabilities2_runner_prompt/semantic_interfaces/semantic_interfaces.yaml index e833931..7fcae68 100644 --- a/capabilities2_runner_prompt/semantic_interfaces/semantic_interfaces.yaml +++ b/capabilities2_runner_prompt/semantic_interfaces/semantic_interfaces.yaml @@ -9,6 +9,6 @@ redefines: capabilities2_runner_prompt/prompt global_namespace: remappings: services: - "prompt": "prompt_text" - "prompt": "prompt_pose" - "prompt": "prompt_occupancy" + "prompt_text": "prompt" + "prompt_pose": "prompt" + "prompt_occupancy": "prompt" diff --git a/capabilities2_server/config/capabilities.yaml b/capabilities2_server/config/capabilities.yaml index f7d19a3..c9e707b 100644 --- a/capabilities2_server/config/capabilities.yaml +++ b/capabilities2_server/config/capabilities.yaml @@ -2,8 +2,9 @@ ros__parameters: loop_rate: 5.0 # Hz db_file: ~/.ros/capabilities/capabilities.sqlite3 - rebuild: false # Set to true to rebuild the database + rebuild: true # Set to true to rebuild the database package_paths: # paths to search for capabilities - /opt/ros/jazzy/share - /home/ubuntu/colcon_ws/src - /home/ubuntu/colcon_ws/src/capabilities2 + - /home/kalana/ROS/capabilities_ws/src/capabilities2 From c33a2fdddd7b4cc8d860e319493461dcad9221c8 Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Tue, 29 Oct 2024 15:52:04 +1100 Subject: [PATCH 012/133] fixed sql safety issue --- capabilities2_runner_nav2/interfaces/map.yaml | 3 ++- .../providers/OccupancyGridRunner.yaml | 2 +- .../providers/RobotPoseRunner.yaml | 2 +- .../providers/WaypointRunner.yaml | 2 +- .../prompt_occupancy_runner.hpp | 2 +- .../prompt_pose_runner.hpp | 2 +- .../prompt_text_runner.hpp | 2 +- capabilities2_runner_prompt/package.xml | 5 ---- .../providers/PromptOccupancyRunner.yaml | 2 +- .../providers/PromptPoseRunner.yaml | 2 +- .../providers/PromptTextRunner.yaml | 2 +- .../semantic_interfaces.yaml | 14 ---------- .../capabilities2_server/models/header.hpp | 3 ++- .../capabilities2_server/models/interface.hpp | 3 ++- .../capabilities2_server/models/provider.hpp | 6 +++-- .../models/semantic_interface.hpp | 3 ++- .../sanitizer/sql_safe.hpp | 27 +++++++++++++++++++ 17 files changed, 48 insertions(+), 34 deletions(-) delete mode 100644 capabilities2_runner_prompt/semantic_interfaces/semantic_interfaces.yaml create mode 100644 capabilities2_server/include/capabilities2_server/sanitizer/sql_safe.hpp diff --git a/capabilities2_runner_nav2/interfaces/map.yaml b/capabilities2_runner_nav2/interfaces/map.yaml index dcfd9c3..9fa6a4a 100644 --- a/capabilities2_runner_nav2/interfaces/map.yaml +++ b/capabilities2_runner_nav2/interfaces/map.yaml @@ -8,7 +8,8 @@ interface: topics: "/map": type: "nav_msgs::msg::OccupancyGrid" - description: "This system allows the extraction of the occupancy grid map produced by the robot's mapping or localization system. + description: | + "This system allows the extraction of the occupancy grid map produced by the robot's mapping or localization system. which enables the decision making authority to understand the environment around the robot. This can be triggered with a '' command. This generally needs to be followed by with a prompting tool event that allows for transfer of data to the decision making authority. This runner will populate the prompting tool's diff --git a/capabilities2_runner_nav2/providers/OccupancyGridRunner.yaml b/capabilities2_runner_nav2/providers/OccupancyGridRunner.yaml index 35a2bce..0c407a9 100644 --- a/capabilities2_runner_nav2/providers/OccupancyGridRunner.yaml +++ b/capabilities2_runner_nav2/providers/OccupancyGridRunner.yaml @@ -4,6 +4,6 @@ name: OccupancyGridRunner spec_type: provider spec_version: 1.1 -description: The capability provider for the robot's occupancy grid map interface +description: "The capability provider for the robot's occupancy grid map interface" implements: capabilities2_runner_nav2/OccupancyGridRunner runner: capabilities2_runner::OccupancyGridRunner diff --git a/capabilities2_runner_nav2/providers/RobotPoseRunner.yaml b/capabilities2_runner_nav2/providers/RobotPoseRunner.yaml index 6de76c0..7e67bc6 100644 --- a/capabilities2_runner_nav2/providers/RobotPoseRunner.yaml +++ b/capabilities2_runner_nav2/providers/RobotPoseRunner.yaml @@ -4,6 +4,6 @@ name: RobotPoseRunner spec_type: provider spec_version: 1.1 -description: The capability provider for the robot's odometry interface +description: "The capability provider for the robot's odometry interface" implements: capabilities2_runner_nav2/RobotPoseRunner runner: capabilities2_runner::RobotPoseRunner diff --git a/capabilities2_runner_nav2/providers/WaypointRunner.yaml b/capabilities2_runner_nav2/providers/WaypointRunner.yaml index e970dd3..4d8a9cd 100644 --- a/capabilities2_runner_nav2/providers/WaypointRunner.yaml +++ b/capabilities2_runner_nav2/providers/WaypointRunner.yaml @@ -4,6 +4,6 @@ name: WaypointRunner spec_type: provider spec_version: 1.1 -description: The capability provider for the follow_waypoint interface +description: "The capability provider for the follow_waypoint interface" implements: capabilities2_runner_nav2/WaypointRunner runner: capabilities2_runner::WayPointRunner diff --git a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_occupancy_runner.hpp b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_occupancy_runner.hpp index d6782e4..afced4a 100644 --- a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_occupancy_runner.hpp +++ b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_occupancy_runner.hpp @@ -30,7 +30,7 @@ class PromptOccupancyRunner : public ServiceRunner */ virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config) override { - init_service(node, run_config, "prompt_occupancy"); + init_service(node, run_config, "prompt"); } /** diff --git a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_pose_runner.hpp b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_pose_runner.hpp index 9556888..f6ca481 100644 --- a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_pose_runner.hpp +++ b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_pose_runner.hpp @@ -30,7 +30,7 @@ class PromptPoseRunner : public ServiceRunner */ virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config) override { - init_service(node, run_config, "prompt_pose"); + init_service(node, run_config, "prompt"); } /** diff --git a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_text_runner.hpp b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_text_runner.hpp index 120c4bc..13713d1 100644 --- a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_text_runner.hpp +++ b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_text_runner.hpp @@ -30,7 +30,7 @@ class PromptTextRunner : public ServiceRunner */ virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config) override { - init_service(node, run_config, "prompt_text"); + init_service(node, run_config, "prompt"); } /** diff --git a/capabilities2_runner_prompt/package.xml b/capabilities2_runner_prompt/package.xml index 162481e..4b80311 100644 --- a/capabilities2_runner_prompt/package.xml +++ b/capabilities2_runner_prompt/package.xml @@ -30,11 +30,6 @@ interfaces/prompt.yaml - - - semantic_interfaces/semantic_interfaces.yaml - - providers/PromptTextRunner.yaml diff --git a/capabilities2_runner_prompt/providers/PromptOccupancyRunner.yaml b/capabilities2_runner_prompt/providers/PromptOccupancyRunner.yaml index 1c33b76..dcf77b1 100644 --- a/capabilities2_runner_prompt/providers/PromptOccupancyRunner.yaml +++ b/capabilities2_runner_prompt/providers/PromptOccupancyRunner.yaml @@ -4,6 +4,6 @@ name: PromptOccupancyRunner spec_type: provider spec_version: 1.1 -description: The capability provider for the prompting interface from prompt tools. This can be used for prompting the occupancy map around the robot. +description: "The capability provider for the prompting interface from prompt tools. This can be used for prompting the occupancy map around the robot." implements: capabilities2_runner_prompt/PromptOccupancyRunner runner: capabilities2_runner::PromptOccupancyRunner diff --git a/capabilities2_runner_prompt/providers/PromptPoseRunner.yaml b/capabilities2_runner_prompt/providers/PromptPoseRunner.yaml index 11adfbe..cc890ef 100644 --- a/capabilities2_runner_prompt/providers/PromptPoseRunner.yaml +++ b/capabilities2_runner_prompt/providers/PromptPoseRunner.yaml @@ -4,6 +4,6 @@ name: PromptPoseRunner spec_type: provider spec_version: 1.1 -description: The capability provider for the prompting interface from prompt tools. This can be used for prompting pose of the robot. +description: "The capability provider for the prompting interface from prompt tools. This can be used for prompting pose of the robot." implements: capabilities2_runner_prompt/PromptPoseRunner runner: capabilities2_runner::PromptPoseRunner diff --git a/capabilities2_runner_prompt/providers/PromptTextRunner.yaml b/capabilities2_runner_prompt/providers/PromptTextRunner.yaml index 98171c8..d10de07 100644 --- a/capabilities2_runner_prompt/providers/PromptTextRunner.yaml +++ b/capabilities2_runner_prompt/providers/PromptTextRunner.yaml @@ -4,6 +4,6 @@ name: PromptTextRunner spec_type: provider spec_version: 1.1 -description: The capability provider for the prompting interface from prompt tools. This can be used for prompting text. +description: "The capability provider for the prompting interface from prompt tools. This can be used for prompting text." implements: capabilities2_runner_prompt/PromptTextRunner runner: capabilities2_runner::PromptTextRunner diff --git a/capabilities2_runner_prompt/semantic_interfaces/semantic_interfaces.yaml b/capabilities2_runner_prompt/semantic_interfaces/semantic_interfaces.yaml deleted file mode 100644 index 7fcae68..0000000 --- a/capabilities2_runner_prompt/semantic_interfaces/semantic_interfaces.yaml +++ /dev/null @@ -1,14 +0,0 @@ -# redefine the empty interface -%YAML 1.1 ---- -name: prompt_semantic_interface -spec_type: semantic_interface -spec_version: 1.1 -description: semantic interface for prompt_tools related runners that allow for multiple impletation towards prompt service -redefines: capabilities2_runner_prompt/prompt -global_namespace: -remappings: - services: - "prompt_text": "prompt" - "prompt_pose": "prompt" - "prompt_occupancy": "prompt" diff --git a/capabilities2_server/include/capabilities2_server/models/header.hpp b/capabilities2_server/include/capabilities2_server/models/header.hpp index 6edd01d..3451957 100644 --- a/capabilities2_server/include/capabilities2_server/models/header.hpp +++ b/capabilities2_server/include/capabilities2_server/models/header.hpp @@ -2,6 +2,7 @@ #include #include +#include namespace capabilities2_server { @@ -70,7 +71,7 @@ struct header_model_t : identifiable_base_t, soft_deleteable_base_t, modifiable_ const std::string to_sql_values() const { - return "'" + name + "', '" + version + "', '" + type + "', '" + description + "'"; + return "'" + name + "', '" + version + "', '" + type + "', '" + to_sql_safe(description) + "'"; } }; diff --git a/capabilities2_server/include/capabilities2_server/models/interface.hpp b/capabilities2_server/include/capabilities2_server/models/interface.hpp index ba5b847..69fa8ab 100644 --- a/capabilities2_server/include/capabilities2_server/models/interface.hpp +++ b/capabilities2_server/include/capabilities2_server/models/interface.hpp @@ -5,6 +5,7 @@ #include #include #include +#include namespace capabilities2_server { @@ -164,7 +165,7 @@ struct interface_model_t : public predicateable_base_t const std::string to_sql_values() const { - return header.to_sql_values() + ", '" + YAML::Dump(interface.to_yaml()) + "'"; + return header.to_sql_values() + ", '" + to_sql_safe(YAML::Dump(interface.to_yaml())) + "'"; } }; diff --git a/capabilities2_server/include/capabilities2_server/models/provider.hpp b/capabilities2_server/include/capabilities2_server/models/provider.hpp index 1c20da6..91269fa 100644 --- a/capabilities2_server/include/capabilities2_server/models/provider.hpp +++ b/capabilities2_server/include/capabilities2_server/models/provider.hpp @@ -6,6 +6,7 @@ #include #include #include +#include namespace capabilities2_server { @@ -75,8 +76,9 @@ struct provider_model_t : public remappable_base_t, predicateable_base_t, public { YAML::Node deps; deps["depends_on"] = depends_on; - return header.to_sql_values() + ", '" + implements + "', '" + YAML::Dump(deps["depends_on"]) + "', '" + - YAML::Dump(remappings.to_yaml()) + "', '" + runner + "', '" + definition_str + "'"; + + return header.to_sql_values() + ", '" + implements + "', '" + to_sql_safe(YAML::Dump(deps["depends_on"])) + "', '" + + to_sql_safe(YAML::Dump(remappings.to_yaml())) + "', '" + runner + "', '" + definition_str + "'"; } }; diff --git a/capabilities2_server/include/capabilities2_server/models/semantic_interface.hpp b/capabilities2_server/include/capabilities2_server/models/semantic_interface.hpp index 482ad13..b479aea 100644 --- a/capabilities2_server/include/capabilities2_server/models/semantic_interface.hpp +++ b/capabilities2_server/include/capabilities2_server/models/semantic_interface.hpp @@ -4,6 +4,7 @@ #include #include #include +#include namespace capabilities2_server { @@ -58,7 +59,7 @@ struct semantic_interface_model_t : public remappable_base_t, predicateable_base std::string to_sql_values() const { return header.to_sql_values() + ", '" + redefines + "', '" + global_namespace + "', '" + - YAML::Dump(remappings.to_yaml()) + "'"; + to_sql_safe(YAML::Dump(remappings.to_yaml())) + "'"; } }; diff --git a/capabilities2_server/include/capabilities2_server/sanitizer/sql_safe.hpp b/capabilities2_server/include/capabilities2_server/sanitizer/sql_safe.hpp new file mode 100644 index 0000000..9e39d77 --- /dev/null +++ b/capabilities2_server/include/capabilities2_server/sanitizer/sql_safe.hpp @@ -0,0 +1,27 @@ +#pragma once +#include +#include + +namespace capabilities2_server +{ + +/** + * @brief convert a generic std::string to a sql safe std::string + * + * @param input generic std::string + * @return sql safe std::string + * + */ +std::string to_sql_safe(const std::string& input) +{ + std::string result = input; + + // Escape backslashes (if necessary for your SQL dialect) + result = std::regex_replace(result, std::regex(R"(\\)"), R"(\\\\)"); + + // Escape single quotes by replacing them with two single quotes + result = std::regex_replace(result, std::regex(R"(')"), R"('')"); + + return result; +} +} // namespace capabilities2_server From de45b178fc2f1281b9069638adb7b82152094b7d Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Tue, 29 Oct 2024 20:44:41 +1100 Subject: [PATCH 013/133] tested sql safety complete --- capabilities2_runner_nav2/interfaces/map.yaml | 3 +-- .../capabilities2_server/capabilities_db.hpp | 15 +++++++------- .../sanitizer/sql_safe.hpp | 20 +++++++++++++++++++ 3 files changed, 29 insertions(+), 9 deletions(-) diff --git a/capabilities2_runner_nav2/interfaces/map.yaml b/capabilities2_runner_nav2/interfaces/map.yaml index 9fa6a4a..dcfd9c3 100644 --- a/capabilities2_runner_nav2/interfaces/map.yaml +++ b/capabilities2_runner_nav2/interfaces/map.yaml @@ -8,8 +8,7 @@ interface: topics: "/map": type: "nav_msgs::msg::OccupancyGrid" - description: | - "This system allows the extraction of the occupancy grid map produced by the robot's mapping or localization system. + description: "This system allows the extraction of the occupancy grid map produced by the robot's mapping or localization system. which enables the decision making authority to understand the environment around the robot. This can be triggered with a '' command. This generally needs to be followed by with a prompting tool event that allows for transfer of data to the decision making authority. This runner will populate the prompting tool's diff --git a/capabilities2_server/include/capabilities2_server/capabilities_db.hpp b/capabilities2_server/include/capabilities2_server/capabilities_db.hpp index db0a059..2a724dd 100644 --- a/capabilities2_server/include/capabilities2_server/capabilities_db.hpp +++ b/capabilities2_server/include/capabilities2_server/capabilities_db.hpp @@ -13,6 +13,7 @@ #include #include #include +#include namespace capabilities2_server { @@ -475,10 +476,10 @@ class CapabilitiesDB : public DBBase interface.header.name = std::string(reinterpret_cast(sqlite3_column_text(stmt, 0))); interface.header.version = sqlite3_column_int(stmt, 1); interface.header.type = std::string(reinterpret_cast(sqlite3_column_text(stmt, 2))); - interface.header.description = std::string(reinterpret_cast(sqlite3_column_text(stmt, 3))); + interface.header.description = from_sql_safe(std::string(reinterpret_cast(sqlite3_column_text(stmt, 3)))); models::specification_model_t spec; - spec.from_yaml(YAML::Load(std::string(reinterpret_cast(sqlite3_column_text(stmt, 4))))); + spec.from_yaml(YAML::Load(from_sql_safe(std::string(reinterpret_cast(sqlite3_column_text(stmt, 4)))))); interface.interface = spec; return interface; @@ -490,11 +491,11 @@ class CapabilitiesDB : public DBBase semantic_interface.header.name = std::string(reinterpret_cast(sqlite3_column_text(stmt, 0))); semantic_interface.header.version = sqlite3_column_int(stmt, 1); semantic_interface.header.type = std::string(reinterpret_cast(sqlite3_column_text(stmt, 2))); - semantic_interface.header.description = std::string(reinterpret_cast(sqlite3_column_text(stmt, 3))); + semantic_interface.header.description = from_sql_safe(std::string(reinterpret_cast(sqlite3_column_text(stmt, 3)))); semantic_interface.redefines = std::string(reinterpret_cast(sqlite3_column_text(stmt, 4))); semantic_interface.global_namespace = std::string(reinterpret_cast(sqlite3_column_text(stmt, 5))); semantic_interface.remappings.from_yaml( - YAML::Load(std::string(reinterpret_cast(sqlite3_column_text(stmt, 6))))); + YAML::Load(from_sql_safe(std::string(reinterpret_cast(sqlite3_column_text(stmt, 6)))))); return semantic_interface; } @@ -505,11 +506,11 @@ class CapabilitiesDB : public DBBase provider.header.name = std::string(reinterpret_cast(sqlite3_column_text(stmt, 0))); provider.header.version = sqlite3_column_int(stmt, 1); provider.header.type = std::string(reinterpret_cast(sqlite3_column_text(stmt, 2))); - provider.header.description = std::string(reinterpret_cast(sqlite3_column_text(stmt, 3))); + provider.header.description = from_sql_safe(std::string(reinterpret_cast(sqlite3_column_text(stmt, 3)))); provider.implements = std::string(reinterpret_cast(sqlite3_column_text(stmt, 4))); - provider.depends_on = YAML::Load(std::string(reinterpret_cast(sqlite3_column_text(stmt, 5)))) + provider.depends_on = YAML::Load(from_sql_safe(std::string(reinterpret_cast(sqlite3_column_text(stmt, 5))))) .as>(); - provider.remappings.from_yaml(YAML::Load(std::string(reinterpret_cast(sqlite3_column_text(stmt, 6))))); + provider.remappings.from_yaml(YAML::Load(from_sql_safe(std::string(reinterpret_cast(sqlite3_column_text(stmt, 6)))))); provider.runner = std::string(reinterpret_cast(sqlite3_column_text(stmt, 7))); return provider; diff --git a/capabilities2_server/include/capabilities2_server/sanitizer/sql_safe.hpp b/capabilities2_server/include/capabilities2_server/sanitizer/sql_safe.hpp index 9e39d77..0f7c2c0 100644 --- a/capabilities2_server/include/capabilities2_server/sanitizer/sql_safe.hpp +++ b/capabilities2_server/include/capabilities2_server/sanitizer/sql_safe.hpp @@ -24,4 +24,24 @@ std::string to_sql_safe(const std::string& input) return result; } + +/** + * @brief convert a sql safe std::string to a generic std::string + * + * @param input sql safe std::string + * @return generic std::string + * + */ +std::string from_sql_safe(const std::string& input) +{ + std::string result = input; + + // Escape backslashes (if necessary for your SQL dialect) + result = std::regex_replace(result, std::regex(R"(\\\\)"), R"(\\)"); + + // Escape single quotes by replacing them with two single quotes + result = std::regex_replace(result, std::regex(R"('')"), R"(')"); + + return result; +} } // namespace capabilities2_server From 3d8c35add43d5c4805e760d00a3746634154c7a4 Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Tue, 29 Oct 2024 22:02:19 +1100 Subject: [PATCH 014/133] updated interface files --- .../{speech_to_text.yaml => ListenerRunner.yaml} | 6 +++--- .../{text_to_speech.yaml => SpeakerRunner.yaml} | 6 +++--- capabilities2_runner_audio/package.xml | 4 ++-- .../interfaces/OccupancyGridRunner.yaml | 16 ++++++++++++++++ .../{pose.yaml => RobotPoseRunner.yaml} | 6 +++--- ...follow_waypoints.yaml => WaypointRunner.yaml} | 6 +++--- capabilities2_runner_nav2/interfaces/map.yaml | 16 ---------------- capabilities2_runner_nav2/package.xml | 6 +++--- .../interfaces/PromptOccupancyRunner.yaml | 13 +++++++++++++ .../interfaces/PromptPoseRunner.yaml | 13 +++++++++++++ .../interfaces/PromptTextRunner.yaml | 13 +++++++++++++ capabilities2_runner_prompt/package.xml | 13 +++++++++++++ 12 files changed, 85 insertions(+), 33 deletions(-) rename capabilities2_runner_audio/interfaces/{speech_to_text.yaml => ListenerRunner.yaml} (87%) rename capabilities2_runner_audio/interfaces/{text_to_speech.yaml => SpeakerRunner.yaml} (67%) create mode 100644 capabilities2_runner_nav2/interfaces/OccupancyGridRunner.yaml rename capabilities2_runner_nav2/interfaces/{pose.yaml => RobotPoseRunner.yaml} (79%) rename capabilities2_runner_nav2/interfaces/{follow_waypoints.yaml => WaypointRunner.yaml} (66%) delete mode 100644 capabilities2_runner_nav2/interfaces/map.yaml create mode 100644 capabilities2_runner_prompt/interfaces/PromptOccupancyRunner.yaml create mode 100644 capabilities2_runner_prompt/interfaces/PromptPoseRunner.yaml create mode 100644 capabilities2_runner_prompt/interfaces/PromptTextRunner.yaml diff --git a/capabilities2_runner_audio/interfaces/speech_to_text.yaml b/capabilities2_runner_audio/interfaces/ListenerRunner.yaml similarity index 87% rename from capabilities2_runner_audio/interfaces/speech_to_text.yaml rename to capabilities2_runner_audio/interfaces/ListenerRunner.yaml index 53f5cde..9c8fa78 100644 --- a/capabilities2_runner_audio/interfaces/speech_to_text.yaml +++ b/capabilities2_runner_audio/interfaces/ListenerRunner.yaml @@ -1,6 +1,6 @@ %YAML 1.1 --- -name: SpeechToText +name: ListenerRunner spec_version: 1.1 spec_type: interface description: "Speech to text capabilities using Audio stack" @@ -10,7 +10,7 @@ interface: type: "hri_audio_msgs::action::SpeechToText" description: "This system allow the robot to listen to sounds in the environment and then interpret that in the text format. This enables the decision making authority to understand the surrounding sounds, discussions regarding or with the robot.The system can - triggered via '' command.This generally needs to be followed by with a + triggered via '' command.This generally needs to be followed by with a prompting tool event that allows for transfer of data to the decision making authority. This runner will populate the prompting tool's execution parameters with the relavent data. As an example if this is followed by prompt_text runner provided by - PromptTextRunner provider, such as ''" \ No newline at end of file + PromptTextRunner provider, such as ''" \ No newline at end of file diff --git a/capabilities2_runner_audio/interfaces/text_to_speech.yaml b/capabilities2_runner_audio/interfaces/SpeakerRunner.yaml similarity index 67% rename from capabilities2_runner_audio/interfaces/text_to_speech.yaml rename to capabilities2_runner_audio/interfaces/SpeakerRunner.yaml index f4ab34c..19887b0 100644 --- a/capabilities2_runner_audio/interfaces/text_to_speech.yaml +++ b/capabilities2_runner_audio/interfaces/SpeakerRunner.yaml @@ -1,6 +1,6 @@ %YAML 1.1 --- -name: TextToSpeech +name: SpeakerRunner spec_version: 1.1 spec_type: interface description: "Text to Speech capabilities using Audio stack" @@ -9,7 +9,7 @@ interface: "/text_to_speech": type: "hri_audio_msgs::action::TextToSpeech" description: "This system allow the robot to speak a given text. The system can triggered via - '' command. where + '' command. where '$value' represents what the robot has to speak. if the robot recieves an command - '', the robot + '', the robot will speak 'how are you'." \ No newline at end of file diff --git a/capabilities2_runner_audio/package.xml b/capabilities2_runner_audio/package.xml index c0386f7..ba87430 100644 --- a/capabilities2_runner_audio/package.xml +++ b/capabilities2_runner_audio/package.xml @@ -28,10 +28,10 @@ ament_cmake - interfaces/speech_to_text.yaml + interfaces/ListenerRunner.yaml - interfaces/text_to_speech.yaml + interfaces/SpeakerRunner.yaml diff --git a/capabilities2_runner_nav2/interfaces/OccupancyGridRunner.yaml b/capabilities2_runner_nav2/interfaces/OccupancyGridRunner.yaml new file mode 100644 index 0000000..5262b9c --- /dev/null +++ b/capabilities2_runner_nav2/interfaces/OccupancyGridRunner.yaml @@ -0,0 +1,16 @@ +%YAML 1.1 +--- +name: OccupancyGridRunner +spec_version: 1.1 +spec_type: interface +description: "Data extraction capability to extract occupancy grid map of the robot" +interface: + topics: + "/map": + type: "nav_msgs::msg::OccupancyGrid" + description: "This system allows the extraction of the occupancy grid map produced by the robot's mapping or localization system. + which enables the decision making authority to understand the environment around the robot. This can be triggered with a + '' command. This generally needs to be followed by with a + prompting tool event that allows for transfer of data to the decision making authority. This runner will populate the + prompting tool's execution parameters with the relavent data. As an example if this is followed by prompt_occupancy + runner provided by PromptOccupancyRunner provider, such as ''." \ No newline at end of file diff --git a/capabilities2_runner_nav2/interfaces/pose.yaml b/capabilities2_runner_nav2/interfaces/RobotPoseRunner.yaml similarity index 79% rename from capabilities2_runner_nav2/interfaces/pose.yaml rename to capabilities2_runner_nav2/interfaces/RobotPoseRunner.yaml index 4fec08c..102894d 100644 --- a/capabilities2_runner_nav2/interfaces/pose.yaml +++ b/capabilities2_runner_nav2/interfaces/RobotPoseRunner.yaml @@ -1,6 +1,6 @@ %YAML 1.1 --- -name: Pose +name: RobotPoseRunner spec_version: 1.1 spec_type: interface description: "data extraction capability to extract pose of the robot" @@ -10,7 +10,7 @@ interface: type: "geometry_msgs::msg::PoseWithCovarianceStamped" description: "This system allows the extraction of the pose of the robot from the odoemtry or localization system. which enables the decision making authority to understand the position and orientation of the robot. This can be triggered with a - '' command. This generally needs to be followed by with a prompting tool + '' command. This generally needs to be followed by with a prompting tool event that allows for transfer of data to the decision making authority. This runner will populate the prompting tool's execution parameters with the relavent data. As an example if this is followed by prompt_pose runner provided by - PromptPoseRunner provider, such as ''" \ No newline at end of file + PromptPoseRunner provider, such as ''" \ No newline at end of file diff --git a/capabilities2_runner_nav2/interfaces/follow_waypoints.yaml b/capabilities2_runner_nav2/interfaces/WaypointRunner.yaml similarity index 66% rename from capabilities2_runner_nav2/interfaces/follow_waypoints.yaml rename to capabilities2_runner_nav2/interfaces/WaypointRunner.yaml index 4e7dcbb..5215964 100644 --- a/capabilities2_runner_nav2/interfaces/follow_waypoints.yaml +++ b/capabilities2_runner_nav2/interfaces/WaypointRunner.yaml @@ -1,6 +1,6 @@ %YAML 1.1 --- -name: FollowWaypoints +name: WaypointRunner spec_version: 1.1 spec_type: interface description: "Waypoint follower capabilities using Nav2 stack" @@ -9,6 +9,6 @@ interface: "/follow_waypoints": type: "nav2_msgs::action::FollowWaypoints" description: "This system allow the robot to navigate to a given two dimensional coordinate - given via '' command. '$value' represents - a value in meters. As an example ' means the + given via '' command. '$value' represents + a value in meters. As an example ' means the robot will move 1.2 meters forward and 0.8 meters to the right side." \ No newline at end of file diff --git a/capabilities2_runner_nav2/interfaces/map.yaml b/capabilities2_runner_nav2/interfaces/map.yaml deleted file mode 100644 index dcfd9c3..0000000 --- a/capabilities2_runner_nav2/interfaces/map.yaml +++ /dev/null @@ -1,16 +0,0 @@ -%YAML 1.1 ---- -name: Map -spec_version: 1.1 -spec_type: interface -description: "data extraction capability to extract occupancy grid map of the robot" -interface: - topics: - "/map": - type: "nav_msgs::msg::OccupancyGrid" - description: "This system allows the extraction of the occupancy grid map produced by the robot's mapping or localization system. - which enables the decision making authority to understand the environment around the robot. This can be triggered with a - '' command. This generally needs to be followed by with a prompting tool - event that allows for transfer of data to the decision making authority. This runner will populate the prompting tool's - execution parameters with the relavent data. As an example if this is followed by prompt_occupancy runner provided by - PromptOccupancyRunner provider, such as ''." \ No newline at end of file diff --git a/capabilities2_runner_nav2/package.xml b/capabilities2_runner_nav2/package.xml index d162138..5de438f 100644 --- a/capabilities2_runner_nav2/package.xml +++ b/capabilities2_runner_nav2/package.xml @@ -28,17 +28,17 @@ - interfaces/follow_waypoints.yaml + interfaces/WaypointRunner.yaml - interfaces/map.yaml + interfaces/OccupancyGridRunner.yaml - interfaces/pose.yaml + interfaces/RobotPoseRunner.yaml diff --git a/capabilities2_runner_prompt/interfaces/PromptOccupancyRunner.yaml b/capabilities2_runner_prompt/interfaces/PromptOccupancyRunner.yaml new file mode 100644 index 0000000..1d921ac --- /dev/null +++ b/capabilities2_runner_prompt/interfaces/PromptOccupancyRunner.yaml @@ -0,0 +1,13 @@ +%YAML 1.1 +--- +name: PromptOccupancyRunner +spec_version: 1.1 +spec_type: interface +description: "This capability focuses on prompting occupancy grid maps to decision making authority." +interface: + services: + "/prompt": + type: "prompt_msgs/srv/Prompt" + description: "The service can be trigged with an xml command such as '' + This needs to be trigged after an '' command so that data + extracted in the OccupancyGridRunner can be prompted using PromptOccupancyRunner to decision making authority." \ No newline at end of file diff --git a/capabilities2_runner_prompt/interfaces/PromptPoseRunner.yaml b/capabilities2_runner_prompt/interfaces/PromptPoseRunner.yaml new file mode 100644 index 0000000..3fcb030 --- /dev/null +++ b/capabilities2_runner_prompt/interfaces/PromptPoseRunner.yaml @@ -0,0 +1,13 @@ +%YAML 1.1 +--- +name: PromptPoseRunner +spec_version: 1.1 +spec_type: interface +description: "This capability focuses on capturing pose (position and orientation) of the robot and transfering them to decision making authority." +interface: + services: + "/prompt": + type: "prompt_msgs/srv/Prompt" + description: "The service can be trigged with an xml command such as '' + This needs to be trigged after an '' command so that data + extracted in the RobotPoseRunner can be prompted using PromptPoseRunner to decision making authority." \ No newline at end of file diff --git a/capabilities2_runner_prompt/interfaces/PromptTextRunner.yaml b/capabilities2_runner_prompt/interfaces/PromptTextRunner.yaml new file mode 100644 index 0000000..b1c9a60 --- /dev/null +++ b/capabilities2_runner_prompt/interfaces/PromptTextRunner.yaml @@ -0,0 +1,13 @@ +%YAML 1.1 +--- +name: PromptTextRunner +spec_version: 1.1 +spec_type: interface +description: "This capability focuses on capturing speech towards robot as text and transfering them to decision making authority." +interface: + services: + "/prompt": + type: "prompt_msgs/srv/Prompt" + description: "The service can be trigged with an xml command such as '' + This needs to be trigged after an '' command so that data + extracted in the ListenerRunner can be prompted using PromptTextRunner to decision making authority." \ No newline at end of file diff --git a/capabilities2_runner_prompt/package.xml b/capabilities2_runner_prompt/package.xml index 4b80311..748db3e 100644 --- a/capabilities2_runner_prompt/package.xml +++ b/capabilities2_runner_prompt/package.xml @@ -30,6 +30,19 @@ interfaces/prompt.yaml + + + interfaces/PromptTextRunner.yaml + + + + interfaces/PromptPoseRunner.yaml + + + + interfaces/PromptOccupancyRunner.yaml + + providers/PromptTextRunner.yaml From 23e41cd6176c2945da00851af9316c8051d9928c Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Wed, 30 Oct 2024 01:28:16 +1100 Subject: [PATCH 015/133] added launch runners that start respective stacks --- capabilities2_runner_audio/CMakeLists.txt | 4 ++++ .../interfaces/AudioListenRunner.yaml | 11 +++++++++++ .../interfaces/AudioSpeakRunner.yaml | 11 +++++++++++ .../interfaces/ListenerRunner.yaml | 14 +++++++------- .../interfaces/SpeakerRunner.yaml | 10 ++++------ .../launch/listen.launch.py | 15 +++++++++++++++ .../launch/speak.launch.py | 15 +++++++++++++++ capabilities2_runner_audio/package.xml | 19 +++++++++++++++++++ .../providers/AudioListenRunner.yaml | 9 +++++++++ .../providers/AudioSpeakRunner.yaml | 9 +++++++++ .../providers/ListenerRunner.yaml | 3 +++ .../providers/SpeakerRunner.yaml | 3 +++ capabilities2_runner_nav2/CMakeLists.txt | 4 ++++ .../interfaces/Nav2Runner.yaml | 17 +++++++++++++++++ .../interfaces/OccupancyGridRunner.yaml | 14 +++++++------- .../interfaces/RobotPoseRunner.yaml | 14 +++++++------- .../interfaces/WaypointRunner.yaml | 10 +++++----- .../launch/nav2stack.launch.py | 15 +++++++++++++++ capabilities2_runner_nav2/package.xml | 14 ++++++++++---- .../providers/Nav2Runner.yaml | 9 +++++++++ .../providers/OccupancyGridRunner.yaml | 3 +++ .../providers/RobotPoseRunner.yaml | 3 +++ .../providers/WaypointRunner.yaml | 3 +++ capabilities2_runner_prompt/CMakeLists.txt | 5 +++++ .../interfaces/PromptOccupancyRunner.yaml | 8 ++++---- .../interfaces/PromptPoseRunner.yaml | 8 ++++---- .../{prompt.yaml => PromptRunner.yaml} | 2 +- .../interfaces/PromptTextRunner.yaml | 8 ++++---- .../launch/prompttool.launch.py | 15 +++++++++++++++ capabilities2_runner_prompt/package.xml | 7 +++++-- .../providers/PromptOccupancyRunner.yaml | 3 +++ .../providers/PromptPoseRunner.yaml | 3 +++ .../providers/PromptRunner.yaml | 9 +++++++++ .../providers/PromptTextRunner.yaml | 3 +++ 34 files changed, 249 insertions(+), 51 deletions(-) create mode 100644 capabilities2_runner_audio/interfaces/AudioListenRunner.yaml create mode 100644 capabilities2_runner_audio/interfaces/AudioSpeakRunner.yaml create mode 100644 capabilities2_runner_audio/launch/listen.launch.py create mode 100644 capabilities2_runner_audio/launch/speak.launch.py create mode 100644 capabilities2_runner_audio/providers/AudioListenRunner.yaml create mode 100644 capabilities2_runner_audio/providers/AudioSpeakRunner.yaml create mode 100644 capabilities2_runner_nav2/interfaces/Nav2Runner.yaml create mode 100644 capabilities2_runner_nav2/launch/nav2stack.launch.py create mode 100644 capabilities2_runner_nav2/providers/Nav2Runner.yaml rename capabilities2_runner_prompt/interfaces/{prompt.yaml => PromptRunner.yaml} (97%) create mode 100644 capabilities2_runner_prompt/launch/prompttool.launch.py create mode 100644 capabilities2_runner_prompt/providers/PromptRunner.yaml diff --git a/capabilities2_runner_audio/CMakeLists.txt b/capabilities2_runner_audio/CMakeLists.txt index 8a7d9dd..bd716d5 100644 --- a/capabilities2_runner_audio/CMakeLists.txt +++ b/capabilities2_runner_audio/CMakeLists.txt @@ -56,6 +56,10 @@ install(DIRECTORY include/ DESTINATION include ) +install(DIRECTORY launch + DESTINATION share/${PROJECT_NAME} +) + install(DIRECTORY interfaces DESTINATION share/${PROJECT_NAME} ) diff --git a/capabilities2_runner_audio/interfaces/AudioListenRunner.yaml b/capabilities2_runner_audio/interfaces/AudioListenRunner.yaml new file mode 100644 index 0000000..2689644 --- /dev/null +++ b/capabilities2_runner_audio/interfaces/AudioListenRunner.yaml @@ -0,0 +1,11 @@ +%YAML 1.1 +--- +name: AudioListenRunner +spec_version: 1.1 +spec_type: interface +description: "This capability implements the basic audio stack and is a dependency for other capabilities that use audio stack functionalities." +interface: + actions: + "/speech_to_text": + type: "hri_audio_msgs::action::SpeechToText" + description: "Speech to text action that listens to the surrounding sounds" \ No newline at end of file diff --git a/capabilities2_runner_audio/interfaces/AudioSpeakRunner.yaml b/capabilities2_runner_audio/interfaces/AudioSpeakRunner.yaml new file mode 100644 index 0000000..91bc899 --- /dev/null +++ b/capabilities2_runner_audio/interfaces/AudioSpeakRunner.yaml @@ -0,0 +1,11 @@ +%YAML 1.1 +--- +name: AudioSpeakRunner +spec_version: 1.1 +spec_type: interface +description: "This capability implements the basic audio stack and is a dependency for other capabilities that use audio stack functionalities." +interface: + actions: + "/text_to_speech": + type: "hri_audio_msgs::action::TextToSpeech" + description: "Text to Speech action that allows speaking the text given" \ No newline at end of file diff --git a/capabilities2_runner_audio/interfaces/ListenerRunner.yaml b/capabilities2_runner_audio/interfaces/ListenerRunner.yaml index 9c8fa78..6db3a9f 100644 --- a/capabilities2_runner_audio/interfaces/ListenerRunner.yaml +++ b/capabilities2_runner_audio/interfaces/ListenerRunner.yaml @@ -3,14 +3,14 @@ name: ListenerRunner spec_version: 1.1 spec_type: interface -description: "Speech to text capabilities using Audio stack" +description: "This capability allow the robot to listen to sounds in the environment and then interpret that in the text format. This enables + the decision making authority to understand the surrounding sounds, discussions regarding or with the robot.The capability can + ve triggered via '' command.This generally needs to be followed by with a + prompting tool event that allows for transfer of data to the decision making authority. This runner will populate the prompting + tool's execution parameters with the relavent data. As an example if this is followed by prompt_text runner provided by + PromptTextRunner provider, such as ''" interface: actions: "/speech_to_text": type: "hri_audio_msgs::action::SpeechToText" - description: "This system allow the robot to listen to sounds in the environment and then interpret that in the text format. This enables - the decision making authority to understand the surrounding sounds, discussions regarding or with the robot.The system can - triggered via '' command.This generally needs to be followed by with a - prompting tool event that allows for transfer of data to the decision making authority. This runner will populate the prompting - tool's execution parameters with the relavent data. As an example if this is followed by prompt_text runner provided by - PromptTextRunner provider, such as ''" \ No newline at end of file + description: "Speech to text action that listens to the surrounding sounds" \ No newline at end of file diff --git a/capabilities2_runner_audio/interfaces/SpeakerRunner.yaml b/capabilities2_runner_audio/interfaces/SpeakerRunner.yaml index 19887b0..9322a08 100644 --- a/capabilities2_runner_audio/interfaces/SpeakerRunner.yaml +++ b/capabilities2_runner_audio/interfaces/SpeakerRunner.yaml @@ -3,13 +3,11 @@ name: SpeakerRunner spec_version: 1.1 spec_type: interface -description: "Text to Speech capabilities using Audio stack" +description: "This capability allow the robot to speak a given text and can be triggered via '' + command. The '$value' represents what the robot has to speak. As an example, if the robot recieves an command in the format of + '', the robot will speak 'how are you'." interface: actions: "/text_to_speech": type: "hri_audio_msgs::action::TextToSpeech" - description: "This system allow the robot to speak a given text. The system can triggered via - '' command. where - '$value' represents what the robot has to speak. if the robot recieves an command - '', the robot - will speak 'how are you'." \ No newline at end of file + description: "Text to Speech action that allows speaking the text given" \ No newline at end of file diff --git a/capabilities2_runner_audio/launch/listen.launch.py b/capabilities2_runner_audio/launch/listen.launch.py new file mode 100644 index 0000000..7d29c1f --- /dev/null +++ b/capabilities2_runner_audio/launch/listen.launch.py @@ -0,0 +1,15 @@ +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from ament_index_python.packages import get_package_share_directory +import os + +def generate_launch_description(): + + listen_launch_path = os.path.join(get_package_share_directory('hri_audio_stack'), 'launch', 'listen.launch.py') + + return LaunchDescription([ + IncludeLaunchDescription( + PythonLaunchDescriptionSource(listen_launch_path), + ) + ]) \ No newline at end of file diff --git a/capabilities2_runner_audio/launch/speak.launch.py b/capabilities2_runner_audio/launch/speak.launch.py new file mode 100644 index 0000000..1663f2b --- /dev/null +++ b/capabilities2_runner_audio/launch/speak.launch.py @@ -0,0 +1,15 @@ +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from ament_index_python.packages import get_package_share_directory +import os + +def generate_launch_description(): + + speak_launch_path = os.path.join(get_package_share_directory('hri_audio_stack'), 'launch', 'speak.launch.py') + + return LaunchDescription([ + IncludeLaunchDescription( + PythonLaunchDescriptionSource(speak_launch_path), + ) + ]) \ No newline at end of file diff --git a/capabilities2_runner_audio/package.xml b/capabilities2_runner_audio/package.xml index ba87430..a9da0bc 100644 --- a/capabilities2_runner_audio/package.xml +++ b/capabilities2_runner_audio/package.xml @@ -21,6 +21,8 @@ tinyxml2 + ros2launch + ament_lint_auto ament_lint_common @@ -30,17 +32,34 @@ interfaces/ListenerRunner.yaml + interfaces/SpeakerRunner.yaml + + interfaces/AudioListenRunner.yaml + + + + interfaces/AudioSpeakRunner.yaml + + providers/ListenerRunner.yaml + providers/SpeakerRunner.yaml + + providers/AudioListenRunner.yaml + + + + providers/AudioSpeakRunner.yaml + diff --git a/capabilities2_runner_audio/providers/AudioListenRunner.yaml b/capabilities2_runner_audio/providers/AudioListenRunner.yaml new file mode 100644 index 0000000..3f26866 --- /dev/null +++ b/capabilities2_runner_audio/providers/AudioListenRunner.yaml @@ -0,0 +1,9 @@ +# the empty provider +%YAML 1.1 +--- +name: AudioListenRunner +spec_type: provider +spec_version: 1.1 +description: "The capability provider for the audio stack" +implements: capabilities2_runner_audio/AudioListenRunner +runner: launch/listen.launch.py diff --git a/capabilities2_runner_audio/providers/AudioSpeakRunner.yaml b/capabilities2_runner_audio/providers/AudioSpeakRunner.yaml new file mode 100644 index 0000000..370cf32 --- /dev/null +++ b/capabilities2_runner_audio/providers/AudioSpeakRunner.yaml @@ -0,0 +1,9 @@ +# the empty provider +%YAML 1.1 +--- +name: AudioSpeakRunner +spec_type: provider +spec_version: 1.1 +description: "The capability provider for the audio stack" +implements: capabilities2_runner_audio/AudioSpeakRunner +runner: launch/speak.launch.py \ No newline at end of file diff --git a/capabilities2_runner_audio/providers/ListenerRunner.yaml b/capabilities2_runner_audio/providers/ListenerRunner.yaml index 0a9429f..b92f1b1 100644 --- a/capabilities2_runner_audio/providers/ListenerRunner.yaml +++ b/capabilities2_runner_audio/providers/ListenerRunner.yaml @@ -7,3 +7,6 @@ spec_version: 1.1 description: The capability provider for the speech_to_text interface implements: capabilities2_runner_audio/ListenerRunner runner: capabilities2_runner::ListenerRunner +depends_on: + "capabilities2_runner_audio/AudioListenRunner": + provider: "capabilities2_runner_audio/AudioListenRunner" \ No newline at end of file diff --git a/capabilities2_runner_audio/providers/SpeakerRunner.yaml b/capabilities2_runner_audio/providers/SpeakerRunner.yaml index 4cd315b..6007d68 100644 --- a/capabilities2_runner_audio/providers/SpeakerRunner.yaml +++ b/capabilities2_runner_audio/providers/SpeakerRunner.yaml @@ -7,3 +7,6 @@ spec_version: 1.1 description: The capability provider for the text_to_speech interface implements: capabilities2_runner_nav2/SpeakerRunner runner: capabilities2_runner::SpeakerRunner +depends_on: + "capabilities2_runner_audio/AudioSpeakRunner": + provider: "capabilities2_runner_audio/AudioSpeakRunner" \ No newline at end of file diff --git a/capabilities2_runner_nav2/CMakeLists.txt b/capabilities2_runner_nav2/CMakeLists.txt index f15e112..fec6194 100644 --- a/capabilities2_runner_nav2/CMakeLists.txt +++ b/capabilities2_runner_nav2/CMakeLists.txt @@ -62,6 +62,10 @@ install(DIRECTORY interfaces DESTINATION share/${PROJECT_NAME} ) +install(DIRECTORY launch + DESTINATION share/${PROJECT_NAME} +) + # install providers files install(DIRECTORY providers DESTINATION share/${PROJECT_NAME} diff --git a/capabilities2_runner_nav2/interfaces/Nav2Runner.yaml b/capabilities2_runner_nav2/interfaces/Nav2Runner.yaml new file mode 100644 index 0000000..bba9a18 --- /dev/null +++ b/capabilities2_runner_nav2/interfaces/Nav2Runner.yaml @@ -0,0 +1,17 @@ +%YAML 1.1 +--- +name: Nav2Runner +spec_version: 1.1 +spec_type: interface +description: "This capability impliments the basic navigation stack and is a dependency for other capabilities that use navigation stack functionalities." +interface: + topics: + "goal": + type: "geometry_msgs/PoseStamped" + description: "Two dimensional navigation goal to which the robot should try to navigate. This goal should have the x and y components of the pose. + position submessage and the z component of the pose.orientation submessage filled out. The goal should be in the /map tf frame, and + the header.frame_id should be set to '/map'." + "cmd_vel": + type: "geometry_msgs/Twist" + description: "Command velocities for a mobile base to follow. Whether or not the command velocities are holonomic is determined by the provider of + this capability and the capabilities on which it depends." diff --git a/capabilities2_runner_nav2/interfaces/OccupancyGridRunner.yaml b/capabilities2_runner_nav2/interfaces/OccupancyGridRunner.yaml index 5262b9c..35ee581 100644 --- a/capabilities2_runner_nav2/interfaces/OccupancyGridRunner.yaml +++ b/capabilities2_runner_nav2/interfaces/OccupancyGridRunner.yaml @@ -3,14 +3,14 @@ name: OccupancyGridRunner spec_version: 1.1 spec_type: interface -description: "Data extraction capability to extract occupancy grid map of the robot" +description: "This capability allows the extraction of the occupancy grid map produced by the robot's mapping or localization system. + which enables the decision making authority to understand the environment around the robot. This can be triggered with a + '' command. This generally needs to be followed by with a + prompting tool event that allows for transfer of data to the decision making authority. This runner will populate the + prompting tool's execution parameters with the relavent data. As an example if this is followed by prompt_occupancy + runner provided by PromptOccupancyRunner provider, such as ''." interface: topics: "/map": type: "nav_msgs::msg::OccupancyGrid" - description: "This system allows the extraction of the occupancy grid map produced by the robot's mapping or localization system. - which enables the decision making authority to understand the environment around the robot. This can be triggered with a - '' command. This generally needs to be followed by with a - prompting tool event that allows for transfer of data to the decision making authority. This runner will populate the - prompting tool's execution parameters with the relavent data. As an example if this is followed by prompt_occupancy - runner provided by PromptOccupancyRunner provider, such as ''." \ No newline at end of file + description: "Standard ros2 message interface containing occupancy grid map of the robot" \ No newline at end of file diff --git a/capabilities2_runner_nav2/interfaces/RobotPoseRunner.yaml b/capabilities2_runner_nav2/interfaces/RobotPoseRunner.yaml index 102894d..9ad3440 100644 --- a/capabilities2_runner_nav2/interfaces/RobotPoseRunner.yaml +++ b/capabilities2_runner_nav2/interfaces/RobotPoseRunner.yaml @@ -3,14 +3,14 @@ name: RobotPoseRunner spec_version: 1.1 spec_type: interface -description: "data extraction capability to extract pose of the robot" +description: "This system allows the extraction of the pose of the robot from the odoemtry or localization system. which enables + the decision making authority to understand the position and orientation of the robot. This can be triggered with a + '' command. This generally needs to be followed by with a prompting tool + event that allows for transfer of data to the decision making authority. This runner will populate the prompting tool's + execution parameters with the relavent data. As an example if this is followed by prompt_pose runner provided by + PromptPoseRunner provider, such as ''" interface: topics: "/pose": type: "geometry_msgs::msg::PoseWithCovarianceStamped" - description: "This system allows the extraction of the pose of the robot from the odoemtry or localization system. which enables - the decision making authority to understand the position and orientation of the robot. This can be triggered with a - '' command. This generally needs to be followed by with a prompting tool - event that allows for transfer of data to the decision making authority. This runner will populate the prompting tool's - execution parameters with the relavent data. As an example if this is followed by prompt_pose runner provided by - PromptPoseRunner provider, such as ''" \ No newline at end of file + description: "Standard ros2 message interface containing position and orientation of the robot" \ No newline at end of file diff --git a/capabilities2_runner_nav2/interfaces/WaypointRunner.yaml b/capabilities2_runner_nav2/interfaces/WaypointRunner.yaml index 5215964..3f71b8b 100644 --- a/capabilities2_runner_nav2/interfaces/WaypointRunner.yaml +++ b/capabilities2_runner_nav2/interfaces/WaypointRunner.yaml @@ -3,12 +3,12 @@ name: WaypointRunner spec_version: 1.1 spec_type: interface -description: "Waypoint follower capabilities using Nav2 stack" +description: "This system allow the robot to navigate to a given two dimensional coordinate + given via '' command. '$value' represents + a value in meters. As an example ' means the + robot will move 1.2 meters forward and 0.8 meters to the right side." interface: actions: "/follow_waypoints": type: "nav2_msgs::action::FollowWaypoints" - description: "This system allow the robot to navigate to a given two dimensional coordinate - given via '' command. '$value' represents - a value in meters. As an example ' means the - robot will move 1.2 meters forward and 0.8 meters to the right side." \ No newline at end of file + description: "Waypoint follower interfacce of the Nav2 stack" \ No newline at end of file diff --git a/capabilities2_runner_nav2/launch/nav2stack.launch.py b/capabilities2_runner_nav2/launch/nav2stack.launch.py new file mode 100644 index 0000000..7190147 --- /dev/null +++ b/capabilities2_runner_nav2/launch/nav2stack.launch.py @@ -0,0 +1,15 @@ +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from ament_index_python.packages import get_package_share_directory +import os + +def generate_launch_description(): + + navstack_launch_path = os.path.join(get_package_share_directory('nav_stack'), 'launch', 'system.launch.py') + + return LaunchDescription([ + IncludeLaunchDescription( + PythonLaunchDescriptionSource(navstack_launch_path), + ) + ]) \ No newline at end of file diff --git a/capabilities2_runner_nav2/package.xml b/capabilities2_runner_nav2/package.xml index 5de438f..681a2d7 100644 --- a/capabilities2_runner_nav2/package.xml +++ b/capabilities2_runner_nav2/package.xml @@ -20,6 +20,8 @@ tinyxml2 + ros2launch + ament_lint_auto ament_lint_common @@ -31,29 +33,33 @@ interfaces/WaypointRunner.yaml - interfaces/OccupancyGridRunner.yaml - interfaces/RobotPoseRunner.yaml + + interfaces/Nav2Runner.yaml + + providers/RobotPoseRunner.yaml - providers/OccupancyGridRunner.yaml - providers/WaypointRunner.yaml + + + providers/Nav2Runner.yaml + diff --git a/capabilities2_runner_nav2/providers/Nav2Runner.yaml b/capabilities2_runner_nav2/providers/Nav2Runner.yaml new file mode 100644 index 0000000..a885d41 --- /dev/null +++ b/capabilities2_runner_nav2/providers/Nav2Runner.yaml @@ -0,0 +1,9 @@ +# the base provider +%YAML 1.1 +--- +name: Nav2Runner +spec_type: provider +spec_version: 1.1 +description: "The capability provider for the nav2 stack" +implements: capabilities2_runner_nav2/Nav2Runner +runner: launch/nav2stack.launch.py diff --git a/capabilities2_runner_nav2/providers/OccupancyGridRunner.yaml b/capabilities2_runner_nav2/providers/OccupancyGridRunner.yaml index 0c407a9..4487e74 100644 --- a/capabilities2_runner_nav2/providers/OccupancyGridRunner.yaml +++ b/capabilities2_runner_nav2/providers/OccupancyGridRunner.yaml @@ -7,3 +7,6 @@ spec_version: 1.1 description: "The capability provider for the robot's occupancy grid map interface" implements: capabilities2_runner_nav2/OccupancyGridRunner runner: capabilities2_runner::OccupancyGridRunner +depends_on: + "capabilities2_runner_nav2/Nav2Runner": + provider: "capabilities2_runner_nav2/Nav2Runner" \ No newline at end of file diff --git a/capabilities2_runner_nav2/providers/RobotPoseRunner.yaml b/capabilities2_runner_nav2/providers/RobotPoseRunner.yaml index 7e67bc6..74b7c7c 100644 --- a/capabilities2_runner_nav2/providers/RobotPoseRunner.yaml +++ b/capabilities2_runner_nav2/providers/RobotPoseRunner.yaml @@ -7,3 +7,6 @@ spec_version: 1.1 description: "The capability provider for the robot's odometry interface" implements: capabilities2_runner_nav2/RobotPoseRunner runner: capabilities2_runner::RobotPoseRunner +depends_on: + "capabilities2_runner_nav2/Nav2Runner": + provider: "capabilities2_runner_nav2/Nav2Runner" \ No newline at end of file diff --git a/capabilities2_runner_nav2/providers/WaypointRunner.yaml b/capabilities2_runner_nav2/providers/WaypointRunner.yaml index 4d8a9cd..cd5f661 100644 --- a/capabilities2_runner_nav2/providers/WaypointRunner.yaml +++ b/capabilities2_runner_nav2/providers/WaypointRunner.yaml @@ -7,3 +7,6 @@ spec_version: 1.1 description: "The capability provider for the follow_waypoint interface" implements: capabilities2_runner_nav2/WaypointRunner runner: capabilities2_runner::WayPointRunner +depends_on: + "capabilities2_runner_nav2/Nav2Runner": + provider: "capabilities2_runner_nav2/Nav2Runner" \ No newline at end of file diff --git a/capabilities2_runner_prompt/CMakeLists.txt b/capabilities2_runner_prompt/CMakeLists.txt index 218496c..7ba17f4 100644 --- a/capabilities2_runner_prompt/CMakeLists.txt +++ b/capabilities2_runner_prompt/CMakeLists.txt @@ -60,6 +60,11 @@ install(DIRECTORY interfaces DESTINATION share/${PROJECT_NAME} ) +# install launch files +install(DIRECTORY launch + DESTINATION share/${PROJECT_NAME} +) + # install providers files install(DIRECTORY providers DESTINATION share/${PROJECT_NAME} diff --git a/capabilities2_runner_prompt/interfaces/PromptOccupancyRunner.yaml b/capabilities2_runner_prompt/interfaces/PromptOccupancyRunner.yaml index 1d921ac..b1aa034 100644 --- a/capabilities2_runner_prompt/interfaces/PromptOccupancyRunner.yaml +++ b/capabilities2_runner_prompt/interfaces/PromptOccupancyRunner.yaml @@ -3,11 +3,11 @@ name: PromptOccupancyRunner spec_version: 1.1 spec_type: interface -description: "This capability focuses on prompting occupancy grid maps to decision making authority." +description: "The capability can be trigged with an command such as '' + This needs to be trigged after an '' command so that data + extracted in the OccupancyGridRunner can be prompted using PromptOccupancyRunner to decision making authority." interface: services: "/prompt": type: "prompt_msgs/srv/Prompt" - description: "The service can be trigged with an xml command such as '' - This needs to be trigged after an '' command so that data - extracted in the OccupancyGridRunner can be prompted using PromptOccupancyRunner to decision making authority." \ No newline at end of file + description: "This service focuses on prompting occupancy grid maps to decision making authority." \ No newline at end of file diff --git a/capabilities2_runner_prompt/interfaces/PromptPoseRunner.yaml b/capabilities2_runner_prompt/interfaces/PromptPoseRunner.yaml index 3fcb030..ebec1e6 100644 --- a/capabilities2_runner_prompt/interfaces/PromptPoseRunner.yaml +++ b/capabilities2_runner_prompt/interfaces/PromptPoseRunner.yaml @@ -3,11 +3,11 @@ name: PromptPoseRunner spec_version: 1.1 spec_type: interface -description: "This capability focuses on capturing pose (position and orientation) of the robot and transfering them to decision making authority." +description: "The capability can be trigged with an command such as '' + This needs to be trigged after an '' command so that data + extracted in the RobotPoseRunner can be prompted using PromptPoseRunner to decision making authority." interface: services: "/prompt": type: "prompt_msgs/srv/Prompt" - description: "The service can be trigged with an xml command such as '' - This needs to be trigged after an '' command so that data - extracted in the RobotPoseRunner can be prompted using PromptPoseRunner to decision making authority." \ No newline at end of file + description: "This service focuses on capturing pose (position and orientation) of the robot and transfering them to decision making authority." \ No newline at end of file diff --git a/capabilities2_runner_prompt/interfaces/prompt.yaml b/capabilities2_runner_prompt/interfaces/PromptRunner.yaml similarity index 97% rename from capabilities2_runner_prompt/interfaces/prompt.yaml rename to capabilities2_runner_prompt/interfaces/PromptRunner.yaml index a537362..e4dbff3 100644 --- a/capabilities2_runner_prompt/interfaces/prompt.yaml +++ b/capabilities2_runner_prompt/interfaces/PromptRunner.yaml @@ -1,6 +1,6 @@ %YAML 1.1 --- -name: Prompt +name: PromptRunner spec_version: 1.1 spec_type: interface description: "This interface describes the basic ROS interface for interacting with the Prompt Bridge." diff --git a/capabilities2_runner_prompt/interfaces/PromptTextRunner.yaml b/capabilities2_runner_prompt/interfaces/PromptTextRunner.yaml index b1c9a60..eb0f53d 100644 --- a/capabilities2_runner_prompt/interfaces/PromptTextRunner.yaml +++ b/capabilities2_runner_prompt/interfaces/PromptTextRunner.yaml @@ -3,11 +3,11 @@ name: PromptTextRunner spec_version: 1.1 spec_type: interface -description: "This capability focuses on capturing speech towards robot as text and transfering them to decision making authority." +description: "The service can be trigged with an command such as '' + This needs to be trigged after an '' command so that data + extracted in the ListenerRunner can be prompted using PromptTextRunner to decision making authority." interface: services: "/prompt": type: "prompt_msgs/srv/Prompt" - description: "The service can be trigged with an xml command such as '' - This needs to be trigged after an '' command so that data - extracted in the ListenerRunner can be prompted using PromptTextRunner to decision making authority." \ No newline at end of file + description: "This capability focuses on capturing speech towards robot as text and transfering them to decision making authority." \ No newline at end of file diff --git a/capabilities2_runner_prompt/launch/prompttool.launch.py b/capabilities2_runner_prompt/launch/prompttool.launch.py new file mode 100644 index 0000000..1594ae3 --- /dev/null +++ b/capabilities2_runner_prompt/launch/prompttool.launch.py @@ -0,0 +1,15 @@ +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from ament_index_python.packages import get_package_share_directory +import os + +def generate_launch_description(): + + prompt_launch_path = os.path.join(get_package_share_directory('prompt_bridge'), 'launch', 'prompt_bridge.launch.py') + + return LaunchDescription([ + IncludeLaunchDescription( + PythonLaunchDescriptionSource(prompt_launch_path), + ) + ]) \ No newline at end of file diff --git a/capabilities2_runner_prompt/package.xml b/capabilities2_runner_prompt/package.xml index 748db3e..102cdee 100644 --- a/capabilities2_runner_prompt/package.xml +++ b/capabilities2_runner_prompt/package.xml @@ -27,10 +27,9 @@ - interfaces/prompt.yaml + interfaces/PromptRunner.yaml - interfaces/PromptTextRunner.yaml @@ -55,5 +54,9 @@ providers/PromptOccupancyRunner.yaml + + + providers/PromptRunner.yaml + diff --git a/capabilities2_runner_prompt/providers/PromptOccupancyRunner.yaml b/capabilities2_runner_prompt/providers/PromptOccupancyRunner.yaml index dcf77b1..4bf7e9b 100644 --- a/capabilities2_runner_prompt/providers/PromptOccupancyRunner.yaml +++ b/capabilities2_runner_prompt/providers/PromptOccupancyRunner.yaml @@ -7,3 +7,6 @@ spec_version: 1.1 description: "The capability provider for the prompting interface from prompt tools. This can be used for prompting the occupancy map around the robot." implements: capabilities2_runner_prompt/PromptOccupancyRunner runner: capabilities2_runner::PromptOccupancyRunner +depends_on: + "capabilities2_runner_prompt/PromptRunner": + provider: "capabilities2_runner_prompt/PromptRunner" \ No newline at end of file diff --git a/capabilities2_runner_prompt/providers/PromptPoseRunner.yaml b/capabilities2_runner_prompt/providers/PromptPoseRunner.yaml index cc890ef..69a4423 100644 --- a/capabilities2_runner_prompt/providers/PromptPoseRunner.yaml +++ b/capabilities2_runner_prompt/providers/PromptPoseRunner.yaml @@ -7,3 +7,6 @@ spec_version: 1.1 description: "The capability provider for the prompting interface from prompt tools. This can be used for prompting pose of the robot." implements: capabilities2_runner_prompt/PromptPoseRunner runner: capabilities2_runner::PromptPoseRunner +depends_on: + "capabilities2_runner_prompt/PromptRunner": + provider: "capabilities2_runner_prompt/PromptRunner" \ No newline at end of file diff --git a/capabilities2_runner_prompt/providers/PromptRunner.yaml b/capabilities2_runner_prompt/providers/PromptRunner.yaml new file mode 100644 index 0000000..102ffd7 --- /dev/null +++ b/capabilities2_runner_prompt/providers/PromptRunner.yaml @@ -0,0 +1,9 @@ +# the base provider +%YAML 1.1 +--- +name: PromptRunner +spec_type: provider +spec_version: 1.1 +description: "The capability provider for the prompt tools" +implements: capabilities2_runner_prompt/PromptRunner +runner: launch/prompttool.launch.py diff --git a/capabilities2_runner_prompt/providers/PromptTextRunner.yaml b/capabilities2_runner_prompt/providers/PromptTextRunner.yaml index d10de07..e773b24 100644 --- a/capabilities2_runner_prompt/providers/PromptTextRunner.yaml +++ b/capabilities2_runner_prompt/providers/PromptTextRunner.yaml @@ -7,3 +7,6 @@ spec_version: 1.1 description: "The capability provider for the prompting interface from prompt tools. This can be used for prompting text." implements: capabilities2_runner_prompt/PromptTextRunner runner: capabilities2_runner::PromptTextRunner +depends_on: + "capabilities2_runner_prompt/PromptRunner": + provider: "capabilities2_runner_prompt/PromptRunner" \ No newline at end of file From 15eb224c088dcbff0212e30614b7cac7cd8d7b9e Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Wed, 30 Oct 2024 14:56:14 +1100 Subject: [PATCH 016/133] renamed launch runners --- .../{AudioListenRunner.yaml => ListenerLaunchRunner.yaml} | 2 +- .../{AudioSpeakRunner.yaml => SpeakerLaunchRunner.yaml} | 2 +- capabilities2_runner_audio/package.xml | 8 ++++---- .../{AudioListenRunner.yaml => ListenerLaunchRunner.yaml} | 4 ++-- capabilities2_runner_audio/providers/ListenerRunner.yaml | 4 ++-- .../{AudioSpeakRunner.yaml => SpeakerLaunchRunner.yaml} | 4 ++-- capabilities2_runner_audio/providers/SpeakerRunner.yaml | 4 ++-- .../interfaces/{Nav2Runner.yaml => Nav2LaunchRunner.yaml} | 2 +- capabilities2_runner_nav2/package.xml | 4 ++-- .../providers/{Nav2Runner.yaml => Nav2LaunchRunner.yaml} | 4 ++-- .../providers/OccupancyGridRunner.yaml | 4 ++-- capabilities2_runner_nav2/providers/RobotPoseRunner.yaml | 4 ++-- capabilities2_runner_nav2/providers/WaypointRunner.yaml | 4 ++-- .../{PromptRunner.yaml => PromptLaunchRunner.yaml} | 2 +- capabilities2_runner_prompt/package.xml | 4 ++-- .../{PromptRunner.yaml => PromptLaunchRunner.yaml} | 4 ++-- .../providers/PromptOccupancyRunner.yaml | 4 ++-- .../providers/PromptPoseRunner.yaml | 4 ++-- .../providers/PromptTextRunner.yaml | 4 ++-- 19 files changed, 36 insertions(+), 36 deletions(-) rename capabilities2_runner_audio/interfaces/{AudioListenRunner.yaml => ListenerLaunchRunner.yaml} (93%) rename capabilities2_runner_audio/interfaces/{AudioSpeakRunner.yaml => SpeakerLaunchRunner.yaml} (93%) rename capabilities2_runner_audio/providers/{AudioListenRunner.yaml => ListenerLaunchRunner.yaml} (65%) rename capabilities2_runner_audio/providers/{AudioSpeakRunner.yaml => SpeakerLaunchRunner.yaml} (65%) rename capabilities2_runner_nav2/interfaces/{Nav2Runner.yaml => Nav2LaunchRunner.yaml} (97%) rename capabilities2_runner_nav2/providers/{Nav2Runner.yaml => Nav2LaunchRunner.yaml} (67%) rename capabilities2_runner_prompt/interfaces/{PromptRunner.yaml => PromptLaunchRunner.yaml} (96%) rename capabilities2_runner_prompt/providers/{PromptRunner.yaml => PromptLaunchRunner.yaml} (66%) diff --git a/capabilities2_runner_audio/interfaces/AudioListenRunner.yaml b/capabilities2_runner_audio/interfaces/ListenerLaunchRunner.yaml similarity index 93% rename from capabilities2_runner_audio/interfaces/AudioListenRunner.yaml rename to capabilities2_runner_audio/interfaces/ListenerLaunchRunner.yaml index 2689644..8402c78 100644 --- a/capabilities2_runner_audio/interfaces/AudioListenRunner.yaml +++ b/capabilities2_runner_audio/interfaces/ListenerLaunchRunner.yaml @@ -1,6 +1,6 @@ %YAML 1.1 --- -name: AudioListenRunner +name: ListenerLaunchRunner spec_version: 1.1 spec_type: interface description: "This capability implements the basic audio stack and is a dependency for other capabilities that use audio stack functionalities." diff --git a/capabilities2_runner_audio/interfaces/AudioSpeakRunner.yaml b/capabilities2_runner_audio/interfaces/SpeakerLaunchRunner.yaml similarity index 93% rename from capabilities2_runner_audio/interfaces/AudioSpeakRunner.yaml rename to capabilities2_runner_audio/interfaces/SpeakerLaunchRunner.yaml index 91bc899..4696ce3 100644 --- a/capabilities2_runner_audio/interfaces/AudioSpeakRunner.yaml +++ b/capabilities2_runner_audio/interfaces/SpeakerLaunchRunner.yaml @@ -1,6 +1,6 @@ %YAML 1.1 --- -name: AudioSpeakRunner +name: SpeakerLaunchRunner spec_version: 1.1 spec_type: interface description: "This capability implements the basic audio stack and is a dependency for other capabilities that use audio stack functionalities." diff --git a/capabilities2_runner_audio/package.xml b/capabilities2_runner_audio/package.xml index a9da0bc..fa38ba8 100644 --- a/capabilities2_runner_audio/package.xml +++ b/capabilities2_runner_audio/package.xml @@ -38,11 +38,11 @@ - interfaces/AudioListenRunner.yaml + interfaces/ListenerLaunchRunner.yaml - interfaces/AudioSpeakRunner.yaml + interfaces/SpeakerLaunchRunner.yaml @@ -55,11 +55,11 @@ - providers/AudioListenRunner.yaml + providers/ListenerLaunchRunner.yaml - providers/AudioSpeakRunner.yaml + providers/SpeakerLaunchRunner.yaml diff --git a/capabilities2_runner_audio/providers/AudioListenRunner.yaml b/capabilities2_runner_audio/providers/ListenerLaunchRunner.yaml similarity index 65% rename from capabilities2_runner_audio/providers/AudioListenRunner.yaml rename to capabilities2_runner_audio/providers/ListenerLaunchRunner.yaml index 3f26866..e747f40 100644 --- a/capabilities2_runner_audio/providers/AudioListenRunner.yaml +++ b/capabilities2_runner_audio/providers/ListenerLaunchRunner.yaml @@ -1,9 +1,9 @@ # the empty provider %YAML 1.1 --- -name: AudioListenRunner +name: ListenerLaunchRunner spec_type: provider spec_version: 1.1 description: "The capability provider for the audio stack" -implements: capabilities2_runner_audio/AudioListenRunner +implements: capabilities2_runner_audio/ListenerLaunchRunner runner: launch/listen.launch.py diff --git a/capabilities2_runner_audio/providers/ListenerRunner.yaml b/capabilities2_runner_audio/providers/ListenerRunner.yaml index b92f1b1..d2e201e 100644 --- a/capabilities2_runner_audio/providers/ListenerRunner.yaml +++ b/capabilities2_runner_audio/providers/ListenerRunner.yaml @@ -8,5 +8,5 @@ description: The capability provider for the speech_to_text interface implements: capabilities2_runner_audio/ListenerRunner runner: capabilities2_runner::ListenerRunner depends_on: - "capabilities2_runner_audio/AudioListenRunner": - provider: "capabilities2_runner_audio/AudioListenRunner" \ No newline at end of file + "capabilities2_runner_audio/ListenerLaunchRunner": + provider: "capabilities2_runner_audio/ListenerLaunchRunner" \ No newline at end of file diff --git a/capabilities2_runner_audio/providers/AudioSpeakRunner.yaml b/capabilities2_runner_audio/providers/SpeakerLaunchRunner.yaml similarity index 65% rename from capabilities2_runner_audio/providers/AudioSpeakRunner.yaml rename to capabilities2_runner_audio/providers/SpeakerLaunchRunner.yaml index 370cf32..7d96b4a 100644 --- a/capabilities2_runner_audio/providers/AudioSpeakRunner.yaml +++ b/capabilities2_runner_audio/providers/SpeakerLaunchRunner.yaml @@ -1,9 +1,9 @@ # the empty provider %YAML 1.1 --- -name: AudioSpeakRunner +name: SpeakerLaunchRunner spec_type: provider spec_version: 1.1 description: "The capability provider for the audio stack" -implements: capabilities2_runner_audio/AudioSpeakRunner +implements: capabilities2_runner_audio/SpeakerLaunchRunner runner: launch/speak.launch.py \ No newline at end of file diff --git a/capabilities2_runner_audio/providers/SpeakerRunner.yaml b/capabilities2_runner_audio/providers/SpeakerRunner.yaml index 6007d68..c5a52f2 100644 --- a/capabilities2_runner_audio/providers/SpeakerRunner.yaml +++ b/capabilities2_runner_audio/providers/SpeakerRunner.yaml @@ -8,5 +8,5 @@ description: The capability provider for the text_to_speech interface implements: capabilities2_runner_nav2/SpeakerRunner runner: capabilities2_runner::SpeakerRunner depends_on: - "capabilities2_runner_audio/AudioSpeakRunner": - provider: "capabilities2_runner_audio/AudioSpeakRunner" \ No newline at end of file + "capabilities2_runner_audio/SpeakerLaunchRunner": + provider: "capabilities2_runner_audio/SpeakerLaunchRunner" \ No newline at end of file diff --git a/capabilities2_runner_nav2/interfaces/Nav2Runner.yaml b/capabilities2_runner_nav2/interfaces/Nav2LaunchRunner.yaml similarity index 97% rename from capabilities2_runner_nav2/interfaces/Nav2Runner.yaml rename to capabilities2_runner_nav2/interfaces/Nav2LaunchRunner.yaml index bba9a18..5048448 100644 --- a/capabilities2_runner_nav2/interfaces/Nav2Runner.yaml +++ b/capabilities2_runner_nav2/interfaces/Nav2LaunchRunner.yaml @@ -1,6 +1,6 @@ %YAML 1.1 --- -name: Nav2Runner +name: Nav2LaunchRunner spec_version: 1.1 spec_type: interface description: "This capability impliments the basic navigation stack and is a dependency for other capabilities that use navigation stack functionalities." diff --git a/capabilities2_runner_nav2/package.xml b/capabilities2_runner_nav2/package.xml index 681a2d7..7af46cb 100644 --- a/capabilities2_runner_nav2/package.xml +++ b/capabilities2_runner_nav2/package.xml @@ -42,7 +42,7 @@ - interfaces/Nav2Runner.yaml + interfaces/Nav2LaunchRunner.yaml @@ -59,7 +59,7 @@ - providers/Nav2Runner.yaml + providers/Nav2LaunchRunner.yaml diff --git a/capabilities2_runner_nav2/providers/Nav2Runner.yaml b/capabilities2_runner_nav2/providers/Nav2LaunchRunner.yaml similarity index 67% rename from capabilities2_runner_nav2/providers/Nav2Runner.yaml rename to capabilities2_runner_nav2/providers/Nav2LaunchRunner.yaml index a885d41..822be89 100644 --- a/capabilities2_runner_nav2/providers/Nav2Runner.yaml +++ b/capabilities2_runner_nav2/providers/Nav2LaunchRunner.yaml @@ -1,9 +1,9 @@ # the base provider %YAML 1.1 --- -name: Nav2Runner +name: Nav2LaunchRunner spec_type: provider spec_version: 1.1 description: "The capability provider for the nav2 stack" -implements: capabilities2_runner_nav2/Nav2Runner +implements: capabilities2_runner_nav2/Nav2LaunchRunner runner: launch/nav2stack.launch.py diff --git a/capabilities2_runner_nav2/providers/OccupancyGridRunner.yaml b/capabilities2_runner_nav2/providers/OccupancyGridRunner.yaml index 4487e74..81b5542 100644 --- a/capabilities2_runner_nav2/providers/OccupancyGridRunner.yaml +++ b/capabilities2_runner_nav2/providers/OccupancyGridRunner.yaml @@ -8,5 +8,5 @@ description: "The capability provider for the robot's occupancy grid map interfa implements: capabilities2_runner_nav2/OccupancyGridRunner runner: capabilities2_runner::OccupancyGridRunner depends_on: - "capabilities2_runner_nav2/Nav2Runner": - provider: "capabilities2_runner_nav2/Nav2Runner" \ No newline at end of file + "capabilities2_runner_nav2/Nav2LaunchRunner": + provider: "capabilities2_runner_nav2/Nav2LaunchRunner" \ No newline at end of file diff --git a/capabilities2_runner_nav2/providers/RobotPoseRunner.yaml b/capabilities2_runner_nav2/providers/RobotPoseRunner.yaml index 74b7c7c..d0572ed 100644 --- a/capabilities2_runner_nav2/providers/RobotPoseRunner.yaml +++ b/capabilities2_runner_nav2/providers/RobotPoseRunner.yaml @@ -8,5 +8,5 @@ description: "The capability provider for the robot's odometry interface" implements: capabilities2_runner_nav2/RobotPoseRunner runner: capabilities2_runner::RobotPoseRunner depends_on: - "capabilities2_runner_nav2/Nav2Runner": - provider: "capabilities2_runner_nav2/Nav2Runner" \ No newline at end of file + "capabilities2_runner_nav2/Nav2LaunchRunner": + provider: "capabilities2_runner_nav2/Nav2LaunchRunner" \ No newline at end of file diff --git a/capabilities2_runner_nav2/providers/WaypointRunner.yaml b/capabilities2_runner_nav2/providers/WaypointRunner.yaml index cd5f661..da5770b 100644 --- a/capabilities2_runner_nav2/providers/WaypointRunner.yaml +++ b/capabilities2_runner_nav2/providers/WaypointRunner.yaml @@ -8,5 +8,5 @@ description: "The capability provider for the follow_waypoint interface" implements: capabilities2_runner_nav2/WaypointRunner runner: capabilities2_runner::WayPointRunner depends_on: - "capabilities2_runner_nav2/Nav2Runner": - provider: "capabilities2_runner_nav2/Nav2Runner" \ No newline at end of file + "capabilities2_runner_nav2/Nav2LaunchRunner": + provider: "capabilities2_runner_nav2/Nav2LaunchRunner" \ No newline at end of file diff --git a/capabilities2_runner_prompt/interfaces/PromptRunner.yaml b/capabilities2_runner_prompt/interfaces/PromptLaunchRunner.yaml similarity index 96% rename from capabilities2_runner_prompt/interfaces/PromptRunner.yaml rename to capabilities2_runner_prompt/interfaces/PromptLaunchRunner.yaml index e4dbff3..c96b2bb 100644 --- a/capabilities2_runner_prompt/interfaces/PromptRunner.yaml +++ b/capabilities2_runner_prompt/interfaces/PromptLaunchRunner.yaml @@ -1,6 +1,6 @@ %YAML 1.1 --- -name: PromptRunner +name: PromptLaunchRunner spec_version: 1.1 spec_type: interface description: "This interface describes the basic ROS interface for interacting with the Prompt Bridge." diff --git a/capabilities2_runner_prompt/package.xml b/capabilities2_runner_prompt/package.xml index 102cdee..5cc4732 100644 --- a/capabilities2_runner_prompt/package.xml +++ b/capabilities2_runner_prompt/package.xml @@ -27,7 +27,7 @@ - interfaces/PromptRunner.yaml + interfaces/PromptLaunchRunner.yaml @@ -56,7 +56,7 @@ - providers/PromptRunner.yaml + providers/PromptLaunchRunner.yaml diff --git a/capabilities2_runner_prompt/providers/PromptRunner.yaml b/capabilities2_runner_prompt/providers/PromptLaunchRunner.yaml similarity index 66% rename from capabilities2_runner_prompt/providers/PromptRunner.yaml rename to capabilities2_runner_prompt/providers/PromptLaunchRunner.yaml index 102ffd7..073588d 100644 --- a/capabilities2_runner_prompt/providers/PromptRunner.yaml +++ b/capabilities2_runner_prompt/providers/PromptLaunchRunner.yaml @@ -1,9 +1,9 @@ # the base provider %YAML 1.1 --- -name: PromptRunner +name: PromptLaunchRunner spec_type: provider spec_version: 1.1 description: "The capability provider for the prompt tools" -implements: capabilities2_runner_prompt/PromptRunner +implements: capabilities2_runner_prompt/PromptLaunchRunner runner: launch/prompttool.launch.py diff --git a/capabilities2_runner_prompt/providers/PromptOccupancyRunner.yaml b/capabilities2_runner_prompt/providers/PromptOccupancyRunner.yaml index 4bf7e9b..6d3f206 100644 --- a/capabilities2_runner_prompt/providers/PromptOccupancyRunner.yaml +++ b/capabilities2_runner_prompt/providers/PromptOccupancyRunner.yaml @@ -8,5 +8,5 @@ description: "The capability provider for the prompting interface from prompt to implements: capabilities2_runner_prompt/PromptOccupancyRunner runner: capabilities2_runner::PromptOccupancyRunner depends_on: - "capabilities2_runner_prompt/PromptRunner": - provider: "capabilities2_runner_prompt/PromptRunner" \ No newline at end of file + "capabilities2_runner_prompt/PromptLaunchRunner": + provider: "capabilities2_runner_prompt/PromptLaunchRunner" \ No newline at end of file diff --git a/capabilities2_runner_prompt/providers/PromptPoseRunner.yaml b/capabilities2_runner_prompt/providers/PromptPoseRunner.yaml index 69a4423..55585e9 100644 --- a/capabilities2_runner_prompt/providers/PromptPoseRunner.yaml +++ b/capabilities2_runner_prompt/providers/PromptPoseRunner.yaml @@ -8,5 +8,5 @@ description: "The capability provider for the prompting interface from prompt to implements: capabilities2_runner_prompt/PromptPoseRunner runner: capabilities2_runner::PromptPoseRunner depends_on: - "capabilities2_runner_prompt/PromptRunner": - provider: "capabilities2_runner_prompt/PromptRunner" \ No newline at end of file + "capabilities2_runner_prompt/PromptLaunchRunner": + provider: "capabilities2_runner_prompt/PromptLaunchRunner" \ No newline at end of file diff --git a/capabilities2_runner_prompt/providers/PromptTextRunner.yaml b/capabilities2_runner_prompt/providers/PromptTextRunner.yaml index e773b24..8741bb4 100644 --- a/capabilities2_runner_prompt/providers/PromptTextRunner.yaml +++ b/capabilities2_runner_prompt/providers/PromptTextRunner.yaml @@ -8,5 +8,5 @@ description: "The capability provider for the prompting interface from prompt to implements: capabilities2_runner_prompt/PromptTextRunner runner: capabilities2_runner::PromptTextRunner depends_on: - "capabilities2_runner_prompt/PromptRunner": - provider: "capabilities2_runner_prompt/PromptRunner" \ No newline at end of file + "capabilities2_runner_prompt/PromptLaunchRunner": + provider: "capabilities2_runner_prompt/PromptLaunchRunner" \ No newline at end of file From a53df36c22b203400b27704b02d5d80d81d574b4 Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Tue, 5 Nov 2024 01:35:36 +1100 Subject: [PATCH 017/133] refactored code --- capabilities2_runner_audio/CMakeLists.txt | 13 -- capabilities2_runner_audio/package.xml | 33 ----- capabilities2_runner_nav2/CMakeLists.txt | 14 -- capabilities2_runner_nav2/package.xml | 36 ----- capabilities2_runner_prompt/CMakeLists.txt | 15 -- .../prompt_occupancy_runner.hpp | 2 +- .../prompt_planner_runner.hpp | 127 +++++++++++++++++ .../prompt_pose_runner.hpp | 2 +- capabilities2_runner_prompt/package.xml | 34 ----- capabilities2_runner_prompt/plugins.xml | 5 + .../src/prompt_runners.cpp | 4 +- std_capabilities2/.clang-format | 64 +++++++++ std_capabilities2/CMakeLists.txt | 24 ++++ .../audio}/ListenerLaunchRunner.yaml | 0 .../interfaces/audio}/ListenerRunner.yaml | 0 .../audio}/SpeakerLaunchRunner.yaml | 0 .../interfaces/audio}/SpeakerRunner.yaml | 0 .../interfaces/nav2}/Nav2LaunchRunner.yaml | 0 .../interfaces/nav2}/OccupancyGridRunner.yaml | 0 .../interfaces/nav2}/RobotPoseRunner.yaml | 0 .../interfaces/nav2}/WaypointRunner.yaml | 0 .../prompt}/PromptLaunchRunner.yaml | 0 .../prompt}/PromptOccupancyRunner.yaml | 0 .../prompt/PromptPlannerRunner.yaml | 22 +++ .../interfaces/prompt}/PromptPoseRunner.yaml | 0 .../interfaces/prompt}/PromptTextRunner.yaml | 0 .../launch/audio}/listen.launch.py | 0 .../launch/audio}/speak.launch.py | 0 .../launch/nav2}/nav2stack.launch.py | 0 .../launch/prompt}/prompttool.launch.py | 0 std_capabilities2/package.xml | 128 ++++++++++++++++++ .../audio}/ListenerLaunchRunner.yaml | 2 +- .../providers/audio}/ListenerRunner.yaml | 0 .../providers/audio}/SpeakerLaunchRunner.yaml | 2 +- .../providers/audio}/SpeakerRunner.yaml | 0 .../providers/nav2}/Nav2LaunchRunner.yaml | 2 +- .../providers/nav2}/OccupancyGridRunner.yaml | 0 .../providers/nav2}/RobotPoseRunner.yaml | 0 .../providers/nav2}/WaypointRunner.yaml | 0 .../providers/prompt}/PromptLaunchRunner.yaml | 2 +- .../prompt}/PromptOccupancyRunner.yaml | 0 .../providers/prompt/PromptPlannerRunner.yaml | 13 ++ .../providers/prompt}/PromptPoseRunner.yaml | 0 .../providers/prompt}/PromptTextRunner.yaml | 0 44 files changed, 392 insertions(+), 152 deletions(-) create mode 100644 capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_planner_runner.hpp create mode 100644 std_capabilities2/.clang-format create mode 100644 std_capabilities2/CMakeLists.txt rename {capabilities2_runner_audio/interfaces => std_capabilities2/interfaces/audio}/ListenerLaunchRunner.yaml (100%) rename {capabilities2_runner_audio/interfaces => std_capabilities2/interfaces/audio}/ListenerRunner.yaml (100%) rename {capabilities2_runner_audio/interfaces => std_capabilities2/interfaces/audio}/SpeakerLaunchRunner.yaml (100%) rename {capabilities2_runner_audio/interfaces => std_capabilities2/interfaces/audio}/SpeakerRunner.yaml (100%) rename {capabilities2_runner_nav2/interfaces => std_capabilities2/interfaces/nav2}/Nav2LaunchRunner.yaml (100%) rename {capabilities2_runner_nav2/interfaces => std_capabilities2/interfaces/nav2}/OccupancyGridRunner.yaml (100%) rename {capabilities2_runner_nav2/interfaces => std_capabilities2/interfaces/nav2}/RobotPoseRunner.yaml (100%) rename {capabilities2_runner_nav2/interfaces => std_capabilities2/interfaces/nav2}/WaypointRunner.yaml (100%) rename {capabilities2_runner_prompt/interfaces => std_capabilities2/interfaces/prompt}/PromptLaunchRunner.yaml (100%) rename {capabilities2_runner_prompt/interfaces => std_capabilities2/interfaces/prompt}/PromptOccupancyRunner.yaml (100%) create mode 100644 std_capabilities2/interfaces/prompt/PromptPlannerRunner.yaml rename {capabilities2_runner_prompt/interfaces => std_capabilities2/interfaces/prompt}/PromptPoseRunner.yaml (100%) rename {capabilities2_runner_prompt/interfaces => std_capabilities2/interfaces/prompt}/PromptTextRunner.yaml (100%) rename {capabilities2_runner_audio/launch => std_capabilities2/launch/audio}/listen.launch.py (100%) rename {capabilities2_runner_audio/launch => std_capabilities2/launch/audio}/speak.launch.py (100%) rename {capabilities2_runner_nav2/launch => std_capabilities2/launch/nav2}/nav2stack.launch.py (100%) rename {capabilities2_runner_prompt/launch => std_capabilities2/launch/prompt}/prompttool.launch.py (100%) create mode 100644 std_capabilities2/package.xml rename {capabilities2_runner_audio/providers => std_capabilities2/providers/audio}/ListenerLaunchRunner.yaml (85%) rename {capabilities2_runner_audio/providers => std_capabilities2/providers/audio}/ListenerRunner.yaml (100%) rename {capabilities2_runner_audio/providers => std_capabilities2/providers/audio}/SpeakerLaunchRunner.yaml (85%) rename {capabilities2_runner_audio/providers => std_capabilities2/providers/audio}/SpeakerRunner.yaml (100%) rename {capabilities2_runner_nav2/providers => std_capabilities2/providers/nav2}/Nav2LaunchRunner.yaml (83%) rename {capabilities2_runner_nav2/providers => std_capabilities2/providers/nav2}/OccupancyGridRunner.yaml (100%) rename {capabilities2_runner_nav2/providers => std_capabilities2/providers/nav2}/RobotPoseRunner.yaml (100%) rename {capabilities2_runner_nav2/providers => std_capabilities2/providers/nav2}/WaypointRunner.yaml (100%) rename {capabilities2_runner_prompt/providers => std_capabilities2/providers/prompt}/PromptLaunchRunner.yaml (83%) rename {capabilities2_runner_prompt/providers => std_capabilities2/providers/prompt}/PromptOccupancyRunner.yaml (100%) create mode 100644 std_capabilities2/providers/prompt/PromptPlannerRunner.yaml rename {capabilities2_runner_prompt/providers => std_capabilities2/providers/prompt}/PromptPoseRunner.yaml (100%) rename {capabilities2_runner_prompt/providers => std_capabilities2/providers/prompt}/PromptTextRunner.yaml (100%) diff --git a/capabilities2_runner_audio/CMakeLists.txt b/capabilities2_runner_audio/CMakeLists.txt index bd716d5..15ba918 100644 --- a/capabilities2_runner_audio/CMakeLists.txt +++ b/capabilities2_runner_audio/CMakeLists.txt @@ -56,19 +56,6 @@ install(DIRECTORY include/ DESTINATION include ) -install(DIRECTORY launch - DESTINATION share/${PROJECT_NAME} -) - -install(DIRECTORY interfaces - DESTINATION share/${PROJECT_NAME} -) - -# install providers files -install(DIRECTORY providers - DESTINATION share/${PROJECT_NAME} -) - ament_export_include_directories(include) ament_export_libraries(${PROJECT_NAME}) diff --git a/capabilities2_runner_audio/package.xml b/capabilities2_runner_audio/package.xml index fa38ba8..1d45b25 100644 --- a/capabilities2_runner_audio/package.xml +++ b/capabilities2_runner_audio/package.xml @@ -28,38 +28,5 @@ ament_cmake - - - interfaces/ListenerRunner.yaml - - - - interfaces/SpeakerRunner.yaml - - - - interfaces/ListenerLaunchRunner.yaml - - - - interfaces/SpeakerLaunchRunner.yaml - - - - - providers/ListenerRunner.yaml - - - - providers/SpeakerRunner.yaml - - - - providers/ListenerLaunchRunner.yaml - - - - providers/SpeakerLaunchRunner.yaml - diff --git a/capabilities2_runner_nav2/CMakeLists.txt b/capabilities2_runner_nav2/CMakeLists.txt index fec6194..3057740 100644 --- a/capabilities2_runner_nav2/CMakeLists.txt +++ b/capabilities2_runner_nav2/CMakeLists.txt @@ -57,20 +57,6 @@ install(DIRECTORY include/ DESTINATION include ) -# install interface files -install(DIRECTORY interfaces - DESTINATION share/${PROJECT_NAME} -) - -install(DIRECTORY launch - DESTINATION share/${PROJECT_NAME} -) - -# install providers files -install(DIRECTORY providers - DESTINATION share/${PROJECT_NAME} -) - ament_export_include_directories(include) ament_export_libraries(${PROJECT_NAME}) diff --git a/capabilities2_runner_nav2/package.xml b/capabilities2_runner_nav2/package.xml index 7af46cb..4f32f58 100644 --- a/capabilities2_runner_nav2/package.xml +++ b/capabilities2_runner_nav2/package.xml @@ -20,46 +20,10 @@ tinyxml2 - ros2launch - ament_lint_auto ament_lint_common ament_cmake - - - - interfaces/WaypointRunner.yaml - - - - interfaces/OccupancyGridRunner.yaml - - - - interfaces/RobotPoseRunner.yaml - - - - interfaces/Nav2LaunchRunner.yaml - - - - - providers/RobotPoseRunner.yaml - - - - providers/OccupancyGridRunner.yaml - - - - providers/WaypointRunner.yaml - - - - providers/Nav2LaunchRunner.yaml - diff --git a/capabilities2_runner_prompt/CMakeLists.txt b/capabilities2_runner_prompt/CMakeLists.txt index 7ba17f4..c82454a 100644 --- a/capabilities2_runner_prompt/CMakeLists.txt +++ b/capabilities2_runner_prompt/CMakeLists.txt @@ -55,21 +55,6 @@ install(DIRECTORY include/ DESTINATION include ) -# install interface files -install(DIRECTORY interfaces - DESTINATION share/${PROJECT_NAME} -) - -# install launch files -install(DIRECTORY launch - DESTINATION share/${PROJECT_NAME} -) - -# install providers files -install(DIRECTORY providers - DESTINATION share/${PROJECT_NAME} -) - ament_export_include_directories(include) ament_export_libraries(${PROJECT_NAME}) diff --git a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_occupancy_runner.hpp b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_occupancy_runner.hpp index afced4a..44e52dc 100644 --- a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_occupancy_runner.hpp +++ b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_occupancy_runner.hpp @@ -13,7 +13,7 @@ namespace capabilities2_runner * * This class is a wrapper around the capabilities2 service runner and is used to * call on the prompt_tools/prompt service, providing it as a capability that prompts - * text values + * occupancy grid map values */ class PromptOccupancyRunner : public ServiceRunner { diff --git a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_planner_runner.hpp b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_planner_runner.hpp new file mode 100644 index 0000000..9a17f8e --- /dev/null +++ b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_planner_runner.hpp @@ -0,0 +1,127 @@ +#pragma once +#include +#include +#include +#include +#include +#include + +namespace capabilities2_runner +{ +/** + * @brief prompt capability runner + * + * This class is a wrapper around the capabilities2 service runner and is used to + * call on the prompt_tools/prompt service, providing it as a capability that prompts + * capabilitie plans values + */ +class PromptPlannerRunner : public ServiceRunner +{ +public: + PromptPlannerRunner() : ServiceRunner() + { + } + + /** + * @brief Starter function for starting the service runner + * + * @param node shared pointer to the capabilities node. Allows to use ros node related functionalities + * @param run_config runner configuration loaded from the yaml file + */ + virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config) override + { + init_service(node, run_config, "prompt"); + } + +protected: + /** + * @brief Generate a request from parameters given. + * + * This function is used in conjunction with the trigger function to inject type erased parameters + * into the typed action + * + * A pattern needs to be implemented in the derived class + * + * @param parameters + * @return prompt_msgs::srv::Prompt::Request the generated request + */ + virtual typename prompt_msgs::srv::Prompt::Request generate_request(tinyxml2::XMLElement* parameters) override + { + parameters_ = parameters; + + tinyxml2::XMLElement* textElement = parameters->FirstChildElement("Text"); + + tinyxml2::XMLPrinter printer; + textElement->Accept(&printer); + + std::string data(printer.CStr()); + + prompt_msgs::srv::Prompt::Request request; + + request.prompt.prompt = "Build a xml plan based on the availbale capabilities to acheive mentioned task. Just " // + " give the xml plan without explanations"; + + prompt_msgs::msg::ModelOption modelOption1; + modelOption1.key = "model"; + modelOption1.value = "llama3.1:8b"; + + request.prompt.options.push_back(modelOption1); + + prompt_msgs::msg::ModelOption modelOption2; + modelOption2.key = "stream"; + modelOption2.value = "false"; + modelOption2.type = prompt_msgs::msg::ModelOption::BOOL_TYPE; + + request.prompt.options.push_back(modelOption2); + + return request; + } + + /** + * @brief generate a typed erased response + * + * this method is used in a callback passed to the trigger caller to get type erased result + * from the service the reponse can be passed by the caller or ignored + * + * The pattern needs to be implemented in the derived class + * + * @param wrapped_result + * @return tinyxml2::XMLElement* + */ + virtual tinyxml2::XMLElement* + generate_response(const prompt_msgs::srv::Prompt::Response::SharedPtr& result) const override + { + document_string = result->response.response; + + return nullptr; + } + + /** + * @brief Update on_success event parameters with new data if avaible. + * + * This function is used to inject new data into the XMLElement containing + * parameters related to the on_success trigger event + * + * A pattern needs to be implemented in the derived class + * + * @param parameters pointer to the XMLElement containing parameters + * @return pointer to the XMLElement containing updated parameters + */ + virtual tinyxml2::XMLElement* update_on_success(tinyxml2::XMLElement* parameters) + { + // Create the Pose element as a child of the existing parameters element + tinyxml2::XMLElement* textElement = parameters->GetDocument()->NewElement("ReceievdPlan"); + parameters->InsertEndChild(textElement); + textElement->SetText(document_string.c_str()); + + // Return the updated parameters element with Pose added + return parameters; + }; + + /** + * Document holder for received xml plan + */ + mutable std::string document_string; +}; + +} // namespace capabilities2_runner diff --git a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_pose_runner.hpp b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_pose_runner.hpp index f6ca481..e26c2ed 100644 --- a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_pose_runner.hpp +++ b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_pose_runner.hpp @@ -13,7 +13,7 @@ namespace capabilities2_runner * * This class is a wrapper around the capabilities2 service runner and is used to * call on the prompt_tools/prompt service, providing it as a capability that prompts - * text values + * robot pose values */ class PromptPoseRunner : public ServiceRunner { diff --git a/capabilities2_runner_prompt/package.xml b/capabilities2_runner_prompt/package.xml index 5cc4732..21697db 100644 --- a/capabilities2_runner_prompt/package.xml +++ b/capabilities2_runner_prompt/package.xml @@ -24,39 +24,5 @@ ament_cmake - - - - interfaces/PromptLaunchRunner.yaml - - - - interfaces/PromptTextRunner.yaml - - - - interfaces/PromptPoseRunner.yaml - - - - interfaces/PromptOccupancyRunner.yaml - - - - - providers/PromptTextRunner.yaml - - - - providers/PromptPoseRunner.yaml - - - - providers/PromptOccupancyRunner.yaml - - - - providers/PromptLaunchRunner.yaml - diff --git a/capabilities2_runner_prompt/plugins.xml b/capabilities2_runner_prompt/plugins.xml index c5ee3ce..f842299 100644 --- a/capabilities2_runner_prompt/plugins.xml +++ b/capabilities2_runner_prompt/plugins.xml @@ -14,4 +14,9 @@ A plugin that provide occupancy grid map prompting capability + + + A plugin that provide execution plan requesting capability + + diff --git a/capabilities2_runner_prompt/src/prompt_runners.cpp b/capabilities2_runner_prompt/src/prompt_runners.cpp index 9bac97c..aaf2a29 100644 --- a/capabilities2_runner_prompt/src/prompt_runners.cpp +++ b/capabilities2_runner_prompt/src/prompt_runners.cpp @@ -3,6 +3,7 @@ #include #include #include +#include namespace capabilities2_runner { @@ -12,4 +13,5 @@ namespace capabilities2_runner // register runner plugins PLUGINLIB_EXPORT_CLASS(capabilities2_runner::PromptTextRunner, capabilities2_runner::RunnerBase) PLUGINLIB_EXPORT_CLASS(capabilities2_runner::PromptPoseRunner, capabilities2_runner::RunnerBase) -PLUGINLIB_EXPORT_CLASS(capabilities2_runner::PromptOccupancyRunner, capabilities2_runner::RunnerBase) \ No newline at end of file +PLUGINLIB_EXPORT_CLASS(capabilities2_runner::PromptOccupancyRunner, capabilities2_runner::RunnerBase) +PLUGINLIB_EXPORT_CLASS(capabilities2_runner::PromptPlannerRunner, capabilities2_runner::RunnerBase) \ No newline at end of file diff --git a/std_capabilities2/.clang-format b/std_capabilities2/.clang-format new file mode 100644 index 0000000..d36804f --- /dev/null +++ b/std_capabilities2/.clang-format @@ -0,0 +1,64 @@ + +BasedOnStyle: Google +AccessModifierOffset: -2 +ConstructorInitializerIndentWidth: 2 +AlignEscapedNewlinesLeft: false +AlignTrailingComments: true +AllowAllParametersOfDeclarationOnNextLine: false +AllowShortIfStatementsOnASingleLine: false +AllowShortLoopsOnASingleLine: false +AllowShortFunctionsOnASingleLine: None +AlwaysBreakTemplateDeclarations: true +AlwaysBreakBeforeMultilineStrings: false +BreakBeforeBinaryOperators: false +BreakBeforeTernaryOperators: false +BreakConstructorInitializersBeforeComma: true +BinPackParameters: true +ColumnLimit: 120 +ConstructorInitializerAllOnOneLineOrOnePerLine: true +DerivePointerBinding: false +PointerBindsToType: true +ExperimentalAutoDetectBinPacking: false +IndentCaseLabels: true +MaxEmptyLinesToKeep: 1 +NamespaceIndentation: None +ObjCSpaceBeforeProtocolList: true +PenaltyBreakBeforeFirstCallParameter: 19 +PenaltyBreakComment: 60 +PenaltyBreakString: 1 +PenaltyBreakFirstLessLess: 1000 +PenaltyExcessCharacter: 1000 +PenaltyReturnTypeOnItsOwnLine: 90 +SpacesBeforeTrailingComments: 2 +Cpp11BracedListStyle: false +Standard: Auto +IndentWidth: 2 +TabWidth: 2 +UseTab: Never +IndentFunctionDeclarationAfterType: false +SpacesInParentheses: false +SpacesInAngles: false +SpaceInEmptyParentheses: false +SpacesInCStyleCastParentheses: false +SpaceAfterControlStatementKeyword: true +SpaceBeforeAssignmentOperators: true +ContinuationIndentWidth: 4 +SortIncludes: false +SpaceAfterCStyleCast: false + +# Configure each individual brace in BraceWrapping +BreakBeforeBraces: Custom + +# Control of individual brace wrapping cases +BraceWrapping: { + AfterClass: 'true' + AfterControlStatement: 'true' + AfterEnum : 'true' + AfterFunction : 'true' + AfterNamespace : 'true' + AfterStruct : 'true' + AfterUnion : 'true' + BeforeCatch : 'true' + BeforeElse : 'true' + IndentBraces : 'false' +} diff --git a/std_capabilities2/CMakeLists.txt b/std_capabilities2/CMakeLists.txt new file mode 100644 index 0000000..bc6e513 --- /dev/null +++ b/std_capabilities2/CMakeLists.txt @@ -0,0 +1,24 @@ +cmake_minimum_required(VERSION 3.8) +project(std_capabilities2) + +find_package(ament_cmake REQUIRED) + +# install interface files +install(DIRECTORY interfaces + DESTINATION share/${PROJECT_NAME} +) + +# install launch files +install(DIRECTORY launch + DESTINATION share/${PROJECT_NAME} +) + +# install providers files +install(DIRECTORY providers + DESTINATION share/${PROJECT_NAME} +) + +ament_export_include_directories(include) +ament_export_libraries(${PROJECT_NAME}) + +ament_package() diff --git a/capabilities2_runner_audio/interfaces/ListenerLaunchRunner.yaml b/std_capabilities2/interfaces/audio/ListenerLaunchRunner.yaml similarity index 100% rename from capabilities2_runner_audio/interfaces/ListenerLaunchRunner.yaml rename to std_capabilities2/interfaces/audio/ListenerLaunchRunner.yaml diff --git a/capabilities2_runner_audio/interfaces/ListenerRunner.yaml b/std_capabilities2/interfaces/audio/ListenerRunner.yaml similarity index 100% rename from capabilities2_runner_audio/interfaces/ListenerRunner.yaml rename to std_capabilities2/interfaces/audio/ListenerRunner.yaml diff --git a/capabilities2_runner_audio/interfaces/SpeakerLaunchRunner.yaml b/std_capabilities2/interfaces/audio/SpeakerLaunchRunner.yaml similarity index 100% rename from capabilities2_runner_audio/interfaces/SpeakerLaunchRunner.yaml rename to std_capabilities2/interfaces/audio/SpeakerLaunchRunner.yaml diff --git a/capabilities2_runner_audio/interfaces/SpeakerRunner.yaml b/std_capabilities2/interfaces/audio/SpeakerRunner.yaml similarity index 100% rename from capabilities2_runner_audio/interfaces/SpeakerRunner.yaml rename to std_capabilities2/interfaces/audio/SpeakerRunner.yaml diff --git a/capabilities2_runner_nav2/interfaces/Nav2LaunchRunner.yaml b/std_capabilities2/interfaces/nav2/Nav2LaunchRunner.yaml similarity index 100% rename from capabilities2_runner_nav2/interfaces/Nav2LaunchRunner.yaml rename to std_capabilities2/interfaces/nav2/Nav2LaunchRunner.yaml diff --git a/capabilities2_runner_nav2/interfaces/OccupancyGridRunner.yaml b/std_capabilities2/interfaces/nav2/OccupancyGridRunner.yaml similarity index 100% rename from capabilities2_runner_nav2/interfaces/OccupancyGridRunner.yaml rename to std_capabilities2/interfaces/nav2/OccupancyGridRunner.yaml diff --git a/capabilities2_runner_nav2/interfaces/RobotPoseRunner.yaml b/std_capabilities2/interfaces/nav2/RobotPoseRunner.yaml similarity index 100% rename from capabilities2_runner_nav2/interfaces/RobotPoseRunner.yaml rename to std_capabilities2/interfaces/nav2/RobotPoseRunner.yaml diff --git a/capabilities2_runner_nav2/interfaces/WaypointRunner.yaml b/std_capabilities2/interfaces/nav2/WaypointRunner.yaml similarity index 100% rename from capabilities2_runner_nav2/interfaces/WaypointRunner.yaml rename to std_capabilities2/interfaces/nav2/WaypointRunner.yaml diff --git a/capabilities2_runner_prompt/interfaces/PromptLaunchRunner.yaml b/std_capabilities2/interfaces/prompt/PromptLaunchRunner.yaml similarity index 100% rename from capabilities2_runner_prompt/interfaces/PromptLaunchRunner.yaml rename to std_capabilities2/interfaces/prompt/PromptLaunchRunner.yaml diff --git a/capabilities2_runner_prompt/interfaces/PromptOccupancyRunner.yaml b/std_capabilities2/interfaces/prompt/PromptOccupancyRunner.yaml similarity index 100% rename from capabilities2_runner_prompt/interfaces/PromptOccupancyRunner.yaml rename to std_capabilities2/interfaces/prompt/PromptOccupancyRunner.yaml diff --git a/std_capabilities2/interfaces/prompt/PromptPlannerRunner.yaml b/std_capabilities2/interfaces/prompt/PromptPlannerRunner.yaml new file mode 100644 index 0000000..fc96d9e --- /dev/null +++ b/std_capabilities2/interfaces/prompt/PromptPlannerRunner.yaml @@ -0,0 +1,22 @@ +%YAML 1.1 +--- +name: PromptPlannerRunner +spec_version: 1.1 +spec_type: interface +description: "The service can be trigged with an command such as ''. + This needs to be trigged at the end of each plan so that decision making authority such as an LLM can be informed to + create a new execution plan depending on the state of the robot. This needs to be followed by another event to transfer the + calculated new plan to the exeutor. An example for that event is ''. + Any and all execution plans calculated by this capability need to be within and xml tags. + Various events within the plan can be organised using sqeuntial, parallel and recovery control flows. Sqeuntial + control flow means events in the sequential control flow will be triggered one after the other on preceding event's success. + Events under sequential control flow need to come within xml tags. + Parallel control flow means events in the parallel control flow will be triggered all at once. Events under parallel + control flow need to come within xml tags. + Recovery control flow means second or additional events will be triggered upon the first event's failure. Events under + recovery control flow need to come within xml tags." +interface: + services: + "/prompt": + type: "prompt_msgs/srv/Prompt" + description: "This capability focuses on capturing speech towards robot as text and transfering them to decision making authority." \ No newline at end of file diff --git a/capabilities2_runner_prompt/interfaces/PromptPoseRunner.yaml b/std_capabilities2/interfaces/prompt/PromptPoseRunner.yaml similarity index 100% rename from capabilities2_runner_prompt/interfaces/PromptPoseRunner.yaml rename to std_capabilities2/interfaces/prompt/PromptPoseRunner.yaml diff --git a/capabilities2_runner_prompt/interfaces/PromptTextRunner.yaml b/std_capabilities2/interfaces/prompt/PromptTextRunner.yaml similarity index 100% rename from capabilities2_runner_prompt/interfaces/PromptTextRunner.yaml rename to std_capabilities2/interfaces/prompt/PromptTextRunner.yaml diff --git a/capabilities2_runner_audio/launch/listen.launch.py b/std_capabilities2/launch/audio/listen.launch.py similarity index 100% rename from capabilities2_runner_audio/launch/listen.launch.py rename to std_capabilities2/launch/audio/listen.launch.py diff --git a/capabilities2_runner_audio/launch/speak.launch.py b/std_capabilities2/launch/audio/speak.launch.py similarity index 100% rename from capabilities2_runner_audio/launch/speak.launch.py rename to std_capabilities2/launch/audio/speak.launch.py diff --git a/capabilities2_runner_nav2/launch/nav2stack.launch.py b/std_capabilities2/launch/nav2/nav2stack.launch.py similarity index 100% rename from capabilities2_runner_nav2/launch/nav2stack.launch.py rename to std_capabilities2/launch/nav2/nav2stack.launch.py diff --git a/capabilities2_runner_prompt/launch/prompttool.launch.py b/std_capabilities2/launch/prompt/prompttool.launch.py similarity index 100% rename from capabilities2_runner_prompt/launch/prompttool.launch.py rename to std_capabilities2/launch/prompt/prompttool.launch.py diff --git a/std_capabilities2/package.xml b/std_capabilities2/package.xml new file mode 100644 index 0000000..0eccf69 --- /dev/null +++ b/std_capabilities2/package.xml @@ -0,0 +1,128 @@ + + + + std_capabilities2 + 0.0.0 + TODO: Package description + kalana + TODO: License declaration + + ament_cmake + + ament_lint_auto + ament_lint_common + + + ament_cmake + + + + interfaces/nav2/WaypointRunner.yaml + + + + interfaces/nav2/OccupancyGridRunner.yaml + + + + interfaces/nav2/RobotPoseRunner.yaml + + + + interfaces/nav2/Nav2LaunchRunner.yaml + + + + + interfaces/prompt/PromptLaunchRunner.yaml + + + + interfaces/prompt/PromptTextRunner.yaml + + + + interfaces/prompt/PromptPoseRunner.yaml + + + + interfaces/prompt/PromptOccupancyRunner.yaml + + + + interfaces/prompt/PromptPlannerRunner.yaml + + + + + interfaces/audio/ListenerRunner.yaml + + + + interfaces/audio/SpeakerRunner.yaml + + + + interfaces/audio/ListenerLaunchRunner.yaml + + + + interfaces/audio/SpeakerLaunchRunner.yaml + + + + + providers/nav2/RobotPoseRunner.yaml + + + + providers/nav2/OccupancyGridRunner.yaml + + + + providers/nav2/WaypointRunner.yaml + + + + providers/nav2/Nav2LaunchRunner.yaml + + + + + providers/prompt/PromptLaunchRunner.yaml + + + + providers/prompt/PromptTextRunner.yaml + + + + providers/prompt/PromptPoseRunner.yaml + + + + providers/prompt/PromptOccupancyRunner.yaml + + + + providers/prompt/PromptPlannerRunner.yaml + + + + + providers/audio/ListenerRunner.yaml + + + + providers/audio/SpeakerRunner.yaml + + + + providers/audio/ListenerLaunchRunner.yaml + + + + providers/audio/SpeakerLaunchRunner.yaml + + + diff --git a/capabilities2_runner_audio/providers/ListenerLaunchRunner.yaml b/std_capabilities2/providers/audio/ListenerLaunchRunner.yaml similarity index 85% rename from capabilities2_runner_audio/providers/ListenerLaunchRunner.yaml rename to std_capabilities2/providers/audio/ListenerLaunchRunner.yaml index e747f40..6321911 100644 --- a/capabilities2_runner_audio/providers/ListenerLaunchRunner.yaml +++ b/std_capabilities2/providers/audio/ListenerLaunchRunner.yaml @@ -6,4 +6,4 @@ spec_type: provider spec_version: 1.1 description: "The capability provider for the audio stack" implements: capabilities2_runner_audio/ListenerLaunchRunner -runner: launch/listen.launch.py +runner: launch/audio/listen.launch.py diff --git a/capabilities2_runner_audio/providers/ListenerRunner.yaml b/std_capabilities2/providers/audio/ListenerRunner.yaml similarity index 100% rename from capabilities2_runner_audio/providers/ListenerRunner.yaml rename to std_capabilities2/providers/audio/ListenerRunner.yaml diff --git a/capabilities2_runner_audio/providers/SpeakerLaunchRunner.yaml b/std_capabilities2/providers/audio/SpeakerLaunchRunner.yaml similarity index 85% rename from capabilities2_runner_audio/providers/SpeakerLaunchRunner.yaml rename to std_capabilities2/providers/audio/SpeakerLaunchRunner.yaml index 7d96b4a..0b2b7db 100644 --- a/capabilities2_runner_audio/providers/SpeakerLaunchRunner.yaml +++ b/std_capabilities2/providers/audio/SpeakerLaunchRunner.yaml @@ -6,4 +6,4 @@ spec_type: provider spec_version: 1.1 description: "The capability provider for the audio stack" implements: capabilities2_runner_audio/SpeakerLaunchRunner -runner: launch/speak.launch.py \ No newline at end of file +runner: launch/audio/speak.launch.py \ No newline at end of file diff --git a/capabilities2_runner_audio/providers/SpeakerRunner.yaml b/std_capabilities2/providers/audio/SpeakerRunner.yaml similarity index 100% rename from capabilities2_runner_audio/providers/SpeakerRunner.yaml rename to std_capabilities2/providers/audio/SpeakerRunner.yaml diff --git a/capabilities2_runner_nav2/providers/Nav2LaunchRunner.yaml b/std_capabilities2/providers/nav2/Nav2LaunchRunner.yaml similarity index 83% rename from capabilities2_runner_nav2/providers/Nav2LaunchRunner.yaml rename to std_capabilities2/providers/nav2/Nav2LaunchRunner.yaml index 822be89..3a297e1 100644 --- a/capabilities2_runner_nav2/providers/Nav2LaunchRunner.yaml +++ b/std_capabilities2/providers/nav2/Nav2LaunchRunner.yaml @@ -6,4 +6,4 @@ spec_type: provider spec_version: 1.1 description: "The capability provider for the nav2 stack" implements: capabilities2_runner_nav2/Nav2LaunchRunner -runner: launch/nav2stack.launch.py +runner: launch/nav2/nav2stack.launch.py diff --git a/capabilities2_runner_nav2/providers/OccupancyGridRunner.yaml b/std_capabilities2/providers/nav2/OccupancyGridRunner.yaml similarity index 100% rename from capabilities2_runner_nav2/providers/OccupancyGridRunner.yaml rename to std_capabilities2/providers/nav2/OccupancyGridRunner.yaml diff --git a/capabilities2_runner_nav2/providers/RobotPoseRunner.yaml b/std_capabilities2/providers/nav2/RobotPoseRunner.yaml similarity index 100% rename from capabilities2_runner_nav2/providers/RobotPoseRunner.yaml rename to std_capabilities2/providers/nav2/RobotPoseRunner.yaml diff --git a/capabilities2_runner_nav2/providers/WaypointRunner.yaml b/std_capabilities2/providers/nav2/WaypointRunner.yaml similarity index 100% rename from capabilities2_runner_nav2/providers/WaypointRunner.yaml rename to std_capabilities2/providers/nav2/WaypointRunner.yaml diff --git a/capabilities2_runner_prompt/providers/PromptLaunchRunner.yaml b/std_capabilities2/providers/prompt/PromptLaunchRunner.yaml similarity index 83% rename from capabilities2_runner_prompt/providers/PromptLaunchRunner.yaml rename to std_capabilities2/providers/prompt/PromptLaunchRunner.yaml index 073588d..0fd1050 100644 --- a/capabilities2_runner_prompt/providers/PromptLaunchRunner.yaml +++ b/std_capabilities2/providers/prompt/PromptLaunchRunner.yaml @@ -6,4 +6,4 @@ spec_type: provider spec_version: 1.1 description: "The capability provider for the prompt tools" implements: capabilities2_runner_prompt/PromptLaunchRunner -runner: launch/prompttool.launch.py +runner: launch/prompt/prompttool.launch.py diff --git a/capabilities2_runner_prompt/providers/PromptOccupancyRunner.yaml b/std_capabilities2/providers/prompt/PromptOccupancyRunner.yaml similarity index 100% rename from capabilities2_runner_prompt/providers/PromptOccupancyRunner.yaml rename to std_capabilities2/providers/prompt/PromptOccupancyRunner.yaml diff --git a/std_capabilities2/providers/prompt/PromptPlannerRunner.yaml b/std_capabilities2/providers/prompt/PromptPlannerRunner.yaml new file mode 100644 index 0000000..9c58e4b --- /dev/null +++ b/std_capabilities2/providers/prompt/PromptPlannerRunner.yaml @@ -0,0 +1,13 @@ +# the empty provider +%YAML 1.1 +--- +name: PromptPlannerRunner +spec_type: provider +spec_version: 1.1 +description: "The capability provider for the prompting for an execution plan. This can be used requesting a new plan at the beginning or when + new data is available" +implements: capabilities2_runner_prompt/PromptPlannerRunner +runner: capabilities2_runner::PromptPlannerRunner +depends_on: + "capabilities2_runner_prompt/PromptLaunchRunner": + provider: "capabilities2_runner_prompt/PromptLaunchRunner" \ No newline at end of file diff --git a/capabilities2_runner_prompt/providers/PromptPoseRunner.yaml b/std_capabilities2/providers/prompt/PromptPoseRunner.yaml similarity index 100% rename from capabilities2_runner_prompt/providers/PromptPoseRunner.yaml rename to std_capabilities2/providers/prompt/PromptPoseRunner.yaml diff --git a/capabilities2_runner_prompt/providers/PromptTextRunner.yaml b/std_capabilities2/providers/prompt/PromptTextRunner.yaml similarity index 100% rename from capabilities2_runner_prompt/providers/PromptTextRunner.yaml rename to std_capabilities2/providers/prompt/PromptTextRunner.yaml From 0270905b361d066d067af03211a0fcc82d9c5cf9 Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Thu, 7 Nov 2024 02:21:08 +1100 Subject: [PATCH 018/133] added and modifiled prompt_plan_x_runners --- .../capabilities_executor.hpp | 46 ++++---- .../capabilities_xml_parser.hpp | 36 ++++-- .../capabilities2_runner/action_runner.hpp | 1 + ...ner.hpp => prompt_plan_request_runner.hpp} | 30 +++-- .../prompt_plan_response_runner.hpp | 105 ++++++++++++++++++ .../src/prompt_runners.cpp | 6 +- ...nner.yaml => PromptPlanRequestRunner.yaml} | 6 +- .../prompt/PromptPlanResponseRunner.yaml | 13 +++ std_capabilities2/package.xml | 12 +- ...nner.yaml => PromptPlanRequestRunner.yaml} | 6 +- .../prompt/PromptPlanResponseRunner.yaml | 12 ++ 11 files changed, 220 insertions(+), 53 deletions(-) rename capabilities2_runner_prompt/include/capabilities2_runner_prompt/{prompt_planner_runner.hpp => prompt_plan_request_runner.hpp} (77%) create mode 100644 capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_response_runner.hpp rename std_capabilities2/interfaces/prompt/{PromptPlannerRunner.yaml => PromptPlanRequestRunner.yaml} (88%) create mode 100644 std_capabilities2/interfaces/prompt/PromptPlanResponseRunner.yaml rename std_capabilities2/providers/prompt/{PromptPlannerRunner.yaml => PromptPlanRequestRunner.yaml} (71%) create mode 100644 std_capabilities2/providers/prompt/PromptPlanResponseRunner.yaml diff --git a/capabilities2_executor/include/capabilities2_executor/capabilities_executor.hpp b/capabilities2_executor/include/capabilities2_executor/capabilities_executor.hpp index 1e9f445..dcc1340 100644 --- a/capabilities2_executor/include/capabilities2_executor/capabilities_executor.hpp +++ b/capabilities2_executor/include/capabilities2_executor/capabilities_executor.hpp @@ -243,7 +243,7 @@ class CapabilitiesExecutor : public rclcpp::Node tinyxml2::XMLElement* plan = capabilities2_xml_parser::get_plan(document); // verify whether the plan is valid - if (!capabilities2_xml_parser::check_tags(plan, interface_list, providers_list, control_tag_list)) + if (!capabilities2_xml_parser::check_tags(plan, interface_list, providers_list, control_tag_list, rejected_list)) { RCLCPP_INFO(this->get_logger(), "Execution plan is faulty. Please recheck and update"); return false; @@ -307,9 +307,26 @@ class CapabilitiesExecutor : public rclcpp::Node { RCLCPP_INFO(this->get_logger(), "Plan verification failed"); - // TODO: improve with error codes - result->success = false; - server_goal_handle->canceled(result); + if (rejected_list.size() > 0) + { + // TODO: improve with error codes + result->success = false; + + for (const auto& rejected_element : rejected_list) + { + RCLCPP_ERROR(this->get_logger(), "Failed Events : %s", rejected_element.c_str()); + result->failed_elements.push_back(rejected_element); + } + + server_goal_handle->canceled(result); + RCLCPP_ERROR(this->get_logger(), "Server Execution Cancelled"); + } + else + { + // TODO: improve with error codes + result->success = false; + server_goal_handle->canceled(result); + } RCLCPP_INFO(this->get_logger(), "Server Execution Cancelled"); } @@ -484,23 +501,7 @@ class CapabilitiesExecutor : public rclcpp::Node return; } - if (result.result->failed_connections.size() > 0) - { - // TODO: improve with error codes - result_out->success = false; - - for (const auto& failed_connection : result.result->failed_connections) - { - RCLCPP_ERROR(this->get_logger(), "Failed Events : %s", failed_connection.source.parameters.c_str()); - - result_out->failed_elements.push_back(failed_connection.source.parameters); - } - - server_goal_handle->canceled(result_out); - - RCLCPP_ERROR(this->get_logger(), "Server Execution Cancelled"); - } - else + if (result.result->failed_connections.size() == 0) { // TODO: improve with error codes result_out->success = true; @@ -543,6 +544,9 @@ class CapabilitiesExecutor : public rclcpp::Node /** Control flow List */ std::vector control_tag_list; + /** Invalid events list */ + std::vector rejected_list; + /** action client for connecting with capabilities server*/ rclcpp_action::Client::SharedPtr client_capabilities_; diff --git a/capabilities2_executor/include/capabilities2_executor/capabilities_xml_parser.hpp b/capabilities2_executor/include/capabilities2_executor/capabilities_xml_parser.hpp index e72318a..81308c2 100644 --- a/capabilities2_executor/include/capabilities2_executor/capabilities_xml_parser.hpp +++ b/capabilities2_executor/include/capabilities2_executor/capabilities_xml_parser.hpp @@ -103,14 +103,15 @@ std::string convert_to_string(tinyxml2::XMLDocument& document) * uses recursive approach to go through the plan * * @param element XML Element to be evaluated - * @param events_list list containing valid event tags - * @param providers_list list containing providers - * @param control_list list containing valid control tags + * @param events list containing valid event tags + * @param providers list containing providers + * @param control list containing valid control tags + * @param rejected list containing invalid tags * * @return `true` if element valid and supported and `false` otherwise */ -bool check_tags(tinyxml2::XMLElement* element, std::vector& events_list, std::vector& providers_list, - std::vector& control_list) +bool check_tags(tinyxml2::XMLElement* element, std::vector& events, std::vector& providers, + std::vector& control, std::vector& rejected) { const char** name; const char** provider; @@ -122,43 +123,56 @@ bool check_tags(tinyxml2::XMLElement* element, std::vector& events_ std::string nametag(*name); std::string providertag(*provider); + std::string parameter_string; + convert_to_string(element, parameter_string); + bool hasChildren = !element->NoChildren(); bool hasSiblings = !capabilities2_xml_parser::isLastElement(element); - bool foundInControl = capabilities2_xml_parser::search(control_list, nametag); - bool foundInEvents = capabilities2_xml_parser::search(events_list, nametag); - bool foundInProviders = capabilities2_xml_parser::search(providers_list, providertag); + bool foundInControl = capabilities2_xml_parser::search(control, nametag); + bool foundInEvents = capabilities2_xml_parser::search(events, nametag); + bool foundInProviders = capabilities2_xml_parser::search(providers, providertag); bool returnValue = true; if (typetag == "Control") { if (foundInControl and hasChildren) - returnValue = returnValue and capabilities2_xml_parser::check_tags(element->FirstChildElement(), events_list, providers_list, control_list); + returnValue = returnValue and capabilities2_xml_parser::check_tags(element->FirstChildElement(), events, providers, control, rejected); if (foundInControl and hasSiblings) - returnValue = returnValue and capabilities2_xml_parser::check_tags(element->NextSiblingElement(), events_list, providers_list, control_list); + returnValue = returnValue and capabilities2_xml_parser::check_tags(element->NextSiblingElement(), events, providers, control, rejected); if (foundInControl and !hasSiblings) returnValue = returnValue; if (!foundInControl) + { + rejected.push_back(parameter_string); return false; + } } else if (typetag == "Event") { if (foundInEvents and foundInProviders and hasSiblings) - returnValue = returnValue and capabilities2_xml_parser::check_tags(element->NextSiblingElement(), events_list, providers_list, control_list); + returnValue = returnValue and capabilities2_xml_parser::check_tags(element->NextSiblingElement(), events, providers, control, rejected); if (foundInEvents and foundInProviders and !hasSiblings) returnValue = returnValue; if (!foundInEvents) + { + rejected.push_back(parameter_string); return false; + } if (!foundInProviders) + { + rejected.push_back(parameter_string); return false; + } } else { + rejected.push_back(parameter_string); return false; } diff --git a/capabilities2_runner/include/capabilities2_runner/action_runner.hpp b/capabilities2_runner/include/capabilities2_runner/action_runner.hpp index 89afed8..04d5d61 100644 --- a/capabilities2_runner/include/capabilities2_runner/action_runner.hpp +++ b/capabilities2_runner/include/capabilities2_runner/action_runner.hpp @@ -166,6 +166,7 @@ class ActionRunner : public RunnerBase // send terminated event if (events[execute_id].on_failure) { + result_ = wrapped_result.result; events[execute_id].on_failure(update_on_failure(events[execute_id].on_failure_param)); execute_id += 1; } diff --git a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_planner_runner.hpp b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_request_runner.hpp similarity index 77% rename from capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_planner_runner.hpp rename to capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_request_runner.hpp index 9a17f8e..f42a945 100644 --- a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_planner_runner.hpp +++ b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_request_runner.hpp @@ -15,10 +15,10 @@ namespace capabilities2_runner * call on the prompt_tools/prompt service, providing it as a capability that prompts * capabilitie plans values */ -class PromptPlannerRunner : public ServiceRunner +class PromptPlanRequestRunner : public ServiceRunner { public: - PromptPlannerRunner() : ServiceRunner() + PromptPlanRequestRunner() : ServiceRunner() { } @@ -49,17 +49,25 @@ class PromptPlannerRunner : public ServiceRunner { parameters_ = parameters; - tinyxml2::XMLElement* textElement = parameters->FirstChildElement("Text"); - - tinyxml2::XMLPrinter printer; - textElement->Accept(&printer); - - std::string data(printer.CStr()); + bool replan; + parameters->QueryBoolAttribute("replan", &replan); prompt_msgs::srv::Prompt::Request request; - request.prompt.prompt = "Build a xml plan based on the availbale capabilities to acheive mentioned task. Just " // - " give the xml plan without explanations"; + if (!replan) + { + request.prompt.prompt = "Build a xml plan based on the availbale capabilities to acheive mentioned " // + "task. Return only the xml plan without explanations or comments"; + } + else + { + tinyxml2::XMLElement* failedElements = parameters->FirstChildElement("FailedElements"); + + request.prompt.prompt = "Rebuild the xml plan based on the availbale capabilities to acheive mentioned " // + "task. Just give the xml plan without explanations or comments. These XML elements " // + "had incompatibilities. " + + std::string(failedElements->GetText()) + "Recorrect them as well"; + } prompt_msgs::msg::ModelOption modelOption1; modelOption1.key = "model"; @@ -109,7 +117,7 @@ class PromptPlannerRunner : public ServiceRunner */ virtual tinyxml2::XMLElement* update_on_success(tinyxml2::XMLElement* parameters) { - // Create the Pose element as a child of the existing parameters element + // Create the plan element as a child of the existing parameters element tinyxml2::XMLElement* textElement = parameters->GetDocument()->NewElement("ReceievdPlan"); parameters->InsertEndChild(textElement); textElement->SetText(document_string.c_str()); diff --git a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_response_runner.hpp b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_response_runner.hpp new file mode 100644 index 0000000..38cb6e2 --- /dev/null +++ b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_response_runner.hpp @@ -0,0 +1,105 @@ +#pragma once + +#include + +#include +#include +#include + +#include +#include + +namespace capabilities2_runner +{ + +/** + * @brief Executor runner class + * + * Class to run capabilities2 executor action based capability + * + */ +class PromptPlanResponseRunner : public ActionRunner +{ +public: + PromptPlanResponseRunner() : 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 + */ + virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config) override + { + init_action(node, run_config, "capabilities"); + } + +protected: + /** + * @brief This generate goal function overrides the generate_goal() function from ActionRunner() + * @param parameters XMLElement that contains parameters in the format + '' + * @return ActionT::Goal the generated goal + */ + virtual capabilities2_msgs::action::Plan::Goal generate_goal(tinyxml2::XMLElement* parameters) override + { + parameters_ = parameters; + + tinyxml2::XMLElement* planElement = parameters->FirstChildElement("ReceievdPlan"); + + auto goal_msg = capabilities2_msgs::action::Plan::Goal(); + + // Check if the element was found and has text content + if (planElement && planElement->GetText()) + { + goal_msg.plan = std::string(planElement->GetText()); + } + + return goal_msg; + } + + /** + * @brief This generate result function overrides the generate_result() function from ActionRunner(). Since + * this is not used in this context, this returns nullptr + * @param result message from FollowWaypoints action + * @return nullptr + */ + virtual tinyxml2::XMLElement* + generate_result(const capabilities2_msgs::action::Plan::Result::SharedPtr& result) override + { + return nullptr; + } + + /** + * @brief Update on_failure event parameters with new data if avaible. + * + * This function is used to inject new data into the XMLElement containing + * parameters related to the on_failure trigger event + * + * A pattern needs to be implemented in the derived class + * + * @param parameters pointer to the XMLElement containing parameters + * @return pointer to the XMLElement containing updated parameters + */ + virtual tinyxml2::XMLElement* update_on_failure(tinyxml2::XMLElement* parameters) + { + parameters->SetAttribute("replan", true); + + // Create the failed elements element as a child of the existing parameters element + tinyxml2::XMLElement* failedElements = parameters->GetDocument()->NewElement("FailedElements"); + parameters->InsertEndChild(failedElements); + + std::string failedElementsString = ""; + + for (const auto& element : result_->failed_elements) + failedElementsString += element; + + failedElements->SetText(failedElementsString.c_str()); + + return parameters; + }; +}; + +} // namespace capabilities2_runner diff --git a/capabilities2_runner_prompt/src/prompt_runners.cpp b/capabilities2_runner_prompt/src/prompt_runners.cpp index aaf2a29..32bc8c4 100644 --- a/capabilities2_runner_prompt/src/prompt_runners.cpp +++ b/capabilities2_runner_prompt/src/prompt_runners.cpp @@ -3,7 +3,8 @@ #include #include #include -#include +#include +#include namespace capabilities2_runner { @@ -14,4 +15,5 @@ namespace capabilities2_runner PLUGINLIB_EXPORT_CLASS(capabilities2_runner::PromptTextRunner, capabilities2_runner::RunnerBase) PLUGINLIB_EXPORT_CLASS(capabilities2_runner::PromptPoseRunner, capabilities2_runner::RunnerBase) PLUGINLIB_EXPORT_CLASS(capabilities2_runner::PromptOccupancyRunner, capabilities2_runner::RunnerBase) -PLUGINLIB_EXPORT_CLASS(capabilities2_runner::PromptPlannerRunner, capabilities2_runner::RunnerBase) \ No newline at end of file +PLUGINLIB_EXPORT_CLASS(capabilities2_runner::PromptPlanRequestRunner, capabilities2_runner::RunnerBase) +PLUGINLIB_EXPORT_CLASS(capabilities2_runner::PromptPlanResponseRunner, capabilities2_runner::RunnerBase) \ No newline at end of file diff --git a/std_capabilities2/interfaces/prompt/PromptPlannerRunner.yaml b/std_capabilities2/interfaces/prompt/PromptPlanRequestRunner.yaml similarity index 88% rename from std_capabilities2/interfaces/prompt/PromptPlannerRunner.yaml rename to std_capabilities2/interfaces/prompt/PromptPlanRequestRunner.yaml index fc96d9e..d66696b 100644 --- a/std_capabilities2/interfaces/prompt/PromptPlannerRunner.yaml +++ b/std_capabilities2/interfaces/prompt/PromptPlanRequestRunner.yaml @@ -1,12 +1,12 @@ %YAML 1.1 --- -name: PromptPlannerRunner +name: PromptPlanRequestRunner spec_version: 1.1 spec_type: interface -description: "The service can be trigged with an command such as ''. +description: "The service can be trigged with an command such as ''. This needs to be trigged at the end of each plan so that decision making authority such as an LLM can be informed to create a new execution plan depending on the state of the robot. This needs to be followed by another event to transfer the - calculated new plan to the exeutor. An example for that event is ''. + calculated new plan to the exeutor. An example for that event is ''. Any and all execution plans calculated by this capability need to be within and xml tags. Various events within the plan can be organised using sqeuntial, parallel and recovery control flows. Sqeuntial control flow means events in the sequential control flow will be triggered one after the other on preceding event's success. diff --git a/std_capabilities2/interfaces/prompt/PromptPlanResponseRunner.yaml b/std_capabilities2/interfaces/prompt/PromptPlanResponseRunner.yaml new file mode 100644 index 0000000..010d05a --- /dev/null +++ b/std_capabilities2/interfaces/prompt/PromptPlanResponseRunner.yaml @@ -0,0 +1,13 @@ +%YAML 1.1 +--- +name: PromptPlanResponseRunner +spec_version: 1.1 +spec_type: interface +description: "The action can be trigged with an command such as ''. + This needs to be trigged at the end of each plan so that the plan created by the decision making authority such as an LLM can + be transferred to the capabilities2 executor for connection extraction and future triggering. " +interface: + services: + "/prompt": + type: "prompt_msgs/srv/Prompt" + description: "This capability focuses on capturing speech towards robot as text and transfering them to decision making authority." \ No newline at end of file diff --git a/std_capabilities2/package.xml b/std_capabilities2/package.xml index 0eccf69..7ee6b7d 100644 --- a/std_capabilities2/package.xml +++ b/std_capabilities2/package.xml @@ -50,7 +50,11 @@ - interfaces/prompt/PromptPlannerRunner.yaml + interfaces/prompt/PromptPlanRequestRunner.yaml + + + + interfaces/prompt/PromptPlanResponseRunner.yaml @@ -105,7 +109,11 @@ - providers/prompt/PromptPlannerRunner.yaml + providers/prompt/PromptPlanRequestRunner.yaml + + + + providers/prompt/PromptPlanResponseRunner.yaml diff --git a/std_capabilities2/providers/prompt/PromptPlannerRunner.yaml b/std_capabilities2/providers/prompt/PromptPlanRequestRunner.yaml similarity index 71% rename from std_capabilities2/providers/prompt/PromptPlannerRunner.yaml rename to std_capabilities2/providers/prompt/PromptPlanRequestRunner.yaml index 9c58e4b..bab68c9 100644 --- a/std_capabilities2/providers/prompt/PromptPlannerRunner.yaml +++ b/std_capabilities2/providers/prompt/PromptPlanRequestRunner.yaml @@ -1,13 +1,13 @@ # the empty provider %YAML 1.1 --- -name: PromptPlannerRunner +name: PromptPlanRequestRunner spec_type: provider spec_version: 1.1 description: "The capability provider for the prompting for an execution plan. This can be used requesting a new plan at the beginning or when new data is available" -implements: capabilities2_runner_prompt/PromptPlannerRunner -runner: capabilities2_runner::PromptPlannerRunner +implements: capabilities2_runner_prompt/PromptPlanRequestRunner +runner: capabilities2_runner::PromptPlanRequestRunner depends_on: "capabilities2_runner_prompt/PromptLaunchRunner": provider: "capabilities2_runner_prompt/PromptLaunchRunner" \ No newline at end of file diff --git a/std_capabilities2/providers/prompt/PromptPlanResponseRunner.yaml b/std_capabilities2/providers/prompt/PromptPlanResponseRunner.yaml new file mode 100644 index 0000000..76410e2 --- /dev/null +++ b/std_capabilities2/providers/prompt/PromptPlanResponseRunner.yaml @@ -0,0 +1,12 @@ +# the empty provider +%YAML 1.1 +--- +name: PromptPlanResponseRunner +spec_type: provider +spec_version: 1.1 +description: "The capability provider for executing the received plan. This can be used to trigger the execution process of the new plan" +implements: capabilities2_runner_prompt/PromptPlanResponseRunner +runner: capabilities2_runner::PromptPlanResponseRunner +depends_on: + "capabilities2_runner_prompt/PromptLaunchRunner": + provider: "capabilities2_runner_prompt/PromptLaunchRunner" \ No newline at end of file From 281c3b5eebf196acc1e510cffc6cba6b1de26e12 Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Thu, 7 Nov 2024 17:09:40 +1100 Subject: [PATCH 019/133] improved interface descriptions --- .../prompt_occupancy_runner.hpp | 5 ++- std_capabilities2/.clang-format | 2 +- .../audio/ListenerLaunchRunner.yaml | 5 ++- .../interfaces/audio/ListenerRunner.yaml | 12 +++--- .../interfaces/audio/SpeakerLaunchRunner.yaml | 5 ++- .../interfaces/audio/SpeakerRunner.yaml | 11 ++++-- .../interfaces/nav2/Nav2LaunchRunner.yaml | 13 ++++--- .../interfaces/nav2/OccupancyGridRunner.yaml | 13 ++++--- .../interfaces/nav2/RobotPoseRunner.yaml | 13 ++++--- .../interfaces/nav2/WaypointRunner.yaml | 10 +++-- .../interfaces/prompt/PromptLaunchRunner.yaml | 3 +- .../prompt/PromptOccupancyRunner.yaml | 10 +++-- .../prompt/PromptPlanRequestRunner.yaml | 39 +++++++++++++------ .../prompt/PromptPlanResponseRunner.yaml | 8 ++-- .../interfaces/prompt/PromptPoseRunner.yaml | 9 +++-- .../interfaces/prompt/PromptTextRunner.yaml | 7 ++-- 16 files changed, 102 insertions(+), 63 deletions(-) diff --git a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_occupancy_runner.hpp b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_occupancy_runner.hpp index 44e52dc..c35fc09 100644 --- a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_occupancy_runner.hpp +++ b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_occupancy_runner.hpp @@ -58,9 +58,10 @@ class PromptOccupancyRunner : public ServiceRunner prompt_msgs::srv::Prompt::Request request; request.prompt.prompt = - "The OccupancyGrid of the robot is given as a ros2 nav_msgs/occupancy_grid of which the content are " + data; + "The OccupancyGrid of the robot shows the surrounding environment of the robot. The data is given as a ros2 // + nav_msgs occupancy_grid of which the content are " + data; - prompt_msgs::msg::ModelOption modelOption1; + prompt_msgs::msg::ModelOption modelOption1; modelOption1.key = "model"; modelOption1.value = "llama3.1:8b"; diff --git a/std_capabilities2/.clang-format b/std_capabilities2/.clang-format index d36804f..92effdd 100644 --- a/std_capabilities2/.clang-format +++ b/std_capabilities2/.clang-format @@ -14,7 +14,7 @@ BreakBeforeBinaryOperators: false BreakBeforeTernaryOperators: false BreakConstructorInitializersBeforeComma: true BinPackParameters: true -ColumnLimit: 120 +ColumnLimit: 150 ConstructorInitializerAllOnOneLineOrOnePerLine: true DerivePointerBinding: false PointerBindsToType: true diff --git a/std_capabilities2/interfaces/audio/ListenerLaunchRunner.yaml b/std_capabilities2/interfaces/audio/ListenerLaunchRunner.yaml index 8402c78..8fac50f 100644 --- a/std_capabilities2/interfaces/audio/ListenerLaunchRunner.yaml +++ b/std_capabilities2/interfaces/audio/ListenerLaunchRunner.yaml @@ -3,9 +3,10 @@ name: ListenerLaunchRunner spec_version: 1.1 spec_type: interface -description: "This capability implements the basic audio stack and is a dependency for other capabilities that use audio stack functionalities." +description: "This capability implements the listener component of the audio stack and is a dependency for other capabilities + that use audio stack functionalities." interface: actions: "/speech_to_text": type: "hri_audio_msgs::action::SpeechToText" - description: "Speech to text action that listens to the surrounding sounds" \ No newline at end of file + description: "Speech to text action that listens to the surrounding sounds" diff --git a/std_capabilities2/interfaces/audio/ListenerRunner.yaml b/std_capabilities2/interfaces/audio/ListenerRunner.yaml index 6db3a9f..c8f35d7 100644 --- a/std_capabilities2/interfaces/audio/ListenerRunner.yaml +++ b/std_capabilities2/interfaces/audio/ListenerRunner.yaml @@ -3,12 +3,12 @@ name: ListenerRunner spec_version: 1.1 spec_type: interface -description: "This capability allow the robot to listen to sounds in the environment and then interpret that in the text format. This enables - the decision making authority to understand the surrounding sounds, discussions regarding or with the robot.The capability can - ve triggered via '' command.This generally needs to be followed by with a - prompting tool event that allows for transfer of data to the decision making authority. This runner will populate the prompting - tool's execution parameters with the relavent data. As an example if this is followed by prompt_text runner provided by - PromptTextRunner provider, such as ''" +description: "This capability depends on the listener capability of the audio stack and allow the robot to listen to sounds in the + environment and then interpret that in the text format. This enables the decision making authority such as an LLM to + understand the surrounding sounds, discussions regarding or with the robot. The capability can be triggered via + '' xml command.This generally needs to be followed by with a text + prompting tool capability such as '' that allows for transfer + of identified audio data as text to the decision making authority so that they can be used for future decision making." interface: actions: "/speech_to_text": diff --git a/std_capabilities2/interfaces/audio/SpeakerLaunchRunner.yaml b/std_capabilities2/interfaces/audio/SpeakerLaunchRunner.yaml index 4696ce3..31258b6 100644 --- a/std_capabilities2/interfaces/audio/SpeakerLaunchRunner.yaml +++ b/std_capabilities2/interfaces/audio/SpeakerLaunchRunner.yaml @@ -3,9 +3,10 @@ name: SpeakerLaunchRunner spec_version: 1.1 spec_type: interface -description: "This capability implements the basic audio stack and is a dependency for other capabilities that use audio stack functionalities." +description: "This capability implements the speaker component of the audio stack and is a dependency for other capabilities + that use audio stack functionalities." interface: actions: "/text_to_speech": type: "hri_audio_msgs::action::TextToSpeech" - description: "Text to Speech action that allows speaking the text given" \ No newline at end of file + description: "Text to Speech action that allows speaking the text given" diff --git a/std_capabilities2/interfaces/audio/SpeakerRunner.yaml b/std_capabilities2/interfaces/audio/SpeakerRunner.yaml index 9322a08..e09b525 100644 --- a/std_capabilities2/interfaces/audio/SpeakerRunner.yaml +++ b/std_capabilities2/interfaces/audio/SpeakerRunner.yaml @@ -3,9 +3,14 @@ name: SpeakerRunner spec_version: 1.1 spec_type: interface -description: "This capability allow the robot to speak a given text and can be triggered via '' - command. The '$value' represents what the robot has to speak. As an example, if the robot recieves an command in the format of - '', the robot will speak 'how are you'." +description: "This capability depends on the speaker capability of the audio stack and allow the robot to speak a given text. This + enables the decision making authority such as an LLM to communicate with voice. The capability can be triggered via + '' xml command. The '$value' represents what the robot + has to speak. As an example, if the robot recieves an command in the format of + '', the robot will speak 'how are you'. If an + response is expected for what the robot speaks, then it is required to have the listening capability triggered after + this capability is triggered successfully. This can be done via '' + xml command." interface: actions: "/text_to_speech": diff --git a/std_capabilities2/interfaces/nav2/Nav2LaunchRunner.yaml b/std_capabilities2/interfaces/nav2/Nav2LaunchRunner.yaml index 5048448..af8c598 100644 --- a/std_capabilities2/interfaces/nav2/Nav2LaunchRunner.yaml +++ b/std_capabilities2/interfaces/nav2/Nav2LaunchRunner.yaml @@ -3,15 +3,16 @@ name: Nav2LaunchRunner spec_version: 1.1 spec_type: interface -description: "This capability impliments the basic navigation stack and is a dependency for other capabilities that use navigation stack functionalities." +description: "This capability impliments the basic navigation stack and is a dependency for other capabilities that depend + on navigation stack functionalities." interface: topics: "goal": type: "geometry_msgs/PoseStamped" - description: "Two dimensional navigation goal to which the robot should try to navigate. This goal should have the x and y components of the pose. - position submessage and the z component of the pose.orientation submessage filled out. The goal should be in the /map tf frame, and - the header.frame_id should be set to '/map'." + description: "Two dimensional navigation goal to which the robot should try to navigate. This goal should have the x + and y components of the pose. position submessage and the z component of the pose.orientation submessage + filled out. The goal should be in the /map tf frame, and the header.frame_id should be set to '/map'." "cmd_vel": type: "geometry_msgs/Twist" - description: "Command velocities for a mobile base to follow. Whether or not the command velocities are holonomic is determined by the provider of - this capability and the capabilities on which it depends." + description: "Command velocities for a mobile base to follow. Whether or not the command velocities are holonomic is + determined by the provider of this capability and the capabilities on which it depends." diff --git a/std_capabilities2/interfaces/nav2/OccupancyGridRunner.yaml b/std_capabilities2/interfaces/nav2/OccupancyGridRunner.yaml index 35ee581..4215d25 100644 --- a/std_capabilities2/interfaces/nav2/OccupancyGridRunner.yaml +++ b/std_capabilities2/interfaces/nav2/OccupancyGridRunner.yaml @@ -3,12 +3,13 @@ name: OccupancyGridRunner spec_version: 1.1 spec_type: interface -description: "This capability allows the extraction of the occupancy grid map produced by the robot's mapping or localization system. - which enables the decision making authority to understand the environment around the robot. This can be triggered with a - '' command. This generally needs to be followed by with a - prompting tool event that allows for transfer of data to the decision making authority. This runner will populate the - prompting tool's execution parameters with the relavent data. As an example if this is followed by prompt_occupancy - runner provided by PromptOccupancyRunner provider, such as ''." +description: "This capability depends on the navigation stack functionalities and allows the extraction of the occupancy grid map + produced by the robot's mapping or localization system. This extracted map can be used by the decision making authority + to understand the environment around the robot as well as possible navigation paths. This capability can be triggered + with '' xml command. Upon the successful execution of this + capability, another capability that allows occupancy grid data transfer to the decision making authority needs to be + triggered. Such a capability can be triggered via '' + xml command." interface: topics: "/map": diff --git a/std_capabilities2/interfaces/nav2/RobotPoseRunner.yaml b/std_capabilities2/interfaces/nav2/RobotPoseRunner.yaml index 9ad3440..49a2871 100644 --- a/std_capabilities2/interfaces/nav2/RobotPoseRunner.yaml +++ b/std_capabilities2/interfaces/nav2/RobotPoseRunner.yaml @@ -3,12 +3,13 @@ name: RobotPoseRunner spec_version: 1.1 spec_type: interface -description: "This system allows the extraction of the pose of the robot from the odoemtry or localization system. which enables - the decision making authority to understand the position and orientation of the robot. This can be triggered with a - '' command. This generally needs to be followed by with a prompting tool - event that allows for transfer of data to the decision making authority. This runner will populate the prompting tool's - execution parameters with the relavent data. As an example if this is followed by prompt_pose runner provided by - PromptPoseRunner provider, such as ''" +description: "This capability depends on the navigation stack functionalities and allows the extraction of the pose of the robot from + the odoemtry or localization system. This extracted robot pose can be used by the decision making authority to understand + the position and orientation of the robot. This capability can be triggered with an xml command as + ''. Upon the successful execution of this capability, another capability + that allows robot pose prompting needs to be triggered for transfer of robot pose data to the decision making authority for + future decision making. '' can be used as an robot pose prompting + capability for this." interface: topics: "/pose": diff --git a/std_capabilities2/interfaces/nav2/WaypointRunner.yaml b/std_capabilities2/interfaces/nav2/WaypointRunner.yaml index 3f71b8b..882ee56 100644 --- a/std_capabilities2/interfaces/nav2/WaypointRunner.yaml +++ b/std_capabilities2/interfaces/nav2/WaypointRunner.yaml @@ -3,10 +3,12 @@ name: WaypointRunner spec_version: 1.1 spec_type: interface -description: "This system allow the robot to navigate to a given two dimensional coordinate - given via '' command. '$value' represents - a value in meters. As an example ' means the - robot will move 1.2 meters forward and 0.8 meters to the right side." +description: "This capability depends on the navigation stack functionalities and allow the robot to navigate to a location given + by two dimensional coordinates. This capability can be used by the decision making authority such as an LLM to move + the robot to a required position and orientation. This capability can be triggered via the xml command + ''. '$value' represents a value in meters. + As an example ' means the robot will move 1.2 + meters forward and 0.8 meters to the right side." interface: actions: "/follow_waypoints": diff --git a/std_capabilities2/interfaces/prompt/PromptLaunchRunner.yaml b/std_capabilities2/interfaces/prompt/PromptLaunchRunner.yaml index c96b2bb..c8e43c3 100644 --- a/std_capabilities2/interfaces/prompt/PromptLaunchRunner.yaml +++ b/std_capabilities2/interfaces/prompt/PromptLaunchRunner.yaml @@ -3,7 +3,8 @@ name: PromptLaunchRunner spec_version: 1.1 spec_type: interface -description: "This interface describes the basic ROS interface for interacting with the Prompt Bridge." +description: "This capability implements the prompt tools stack and is a dependency for other capabilities + that use prompting functionalities." interface: topics: "/history": diff --git a/std_capabilities2/interfaces/prompt/PromptOccupancyRunner.yaml b/std_capabilities2/interfaces/prompt/PromptOccupancyRunner.yaml index b1aa034..abc09cf 100644 --- a/std_capabilities2/interfaces/prompt/PromptOccupancyRunner.yaml +++ b/std_capabilities2/interfaces/prompt/PromptOccupancyRunner.yaml @@ -3,9 +3,13 @@ name: PromptOccupancyRunner spec_version: 1.1 spec_type: interface -description: "The capability can be trigged with an command such as '' - This needs to be trigged after an '' command so that data - extracted in the OccupancyGridRunner can be prompted using PromptOccupancyRunner to decision making authority." +description: "This capability depends on the prompt tools stack functionalities and allows an decision making authority such as an + LLM to recieve occupancy grid data from the robot. This capability can be trigged with the xml command + ''. This will prompt the nav_msgs/occupancy_grid + msg in the xml format and would allow the decision making authority to understand the surrounding environment of the + robot. Every execution plan need to have this as one of the last and compulsory capabilities to be executed this at + the end, so that decision making authority is updated about the latest environment information which can then be used + for future planning." interface: services: "/prompt": diff --git a/std_capabilities2/interfaces/prompt/PromptPlanRequestRunner.yaml b/std_capabilities2/interfaces/prompt/PromptPlanRequestRunner.yaml index d66696b..f2cba36 100644 --- a/std_capabilities2/interfaces/prompt/PromptPlanRequestRunner.yaml +++ b/std_capabilities2/interfaces/prompt/PromptPlanRequestRunner.yaml @@ -3,18 +3,33 @@ name: PromptPlanRequestRunner spec_version: 1.1 spec_type: interface -description: "The service can be trigged with an command such as ''. - This needs to be trigged at the end of each plan so that decision making authority such as an LLM can be informed to - create a new execution plan depending on the state of the robot. This needs to be followed by another event to transfer the - calculated new plan to the exeutor. An example for that event is ''. - Any and all execution plans calculated by this capability need to be within and xml tags. - Various events within the plan can be organised using sqeuntial, parallel and recovery control flows. Sqeuntial - control flow means events in the sequential control flow will be triggered one after the other on preceding event's success. - Events under sequential control flow need to come within xml tags. - Parallel control flow means events in the parallel control flow will be triggered all at once. Events under parallel - control flow need to come within xml tags. - Recovery control flow means second or additional events will be triggered upon the first event's failure. Events under - recovery control flow need to come within xml tags." +description: "This capability depends on the prompt tools stack functionalities and allows an decision making authority such as an + LLM to recieve a request to create a new plan at the end of an execution plan. This capability can can be trigged with + an xml command such as ''. This will + prompt the decision making authority to create a new execution plan either to continue the current plan or to focus on + a new task depending on the state of the robot. This capability needs to be in every execution plan created and needs + be the capability before the last in any plan. Any and all execution plans calculated by this capability need to be + contained within and xml tags. Capabilities within the execution plan can be organised using sqeuntial, + parallel and recovery control flows. Capabilities in the sequential control flow will be triggered one after the other + depending on precceding capability's successful execution. Capabilities under sequential control flow need to be contained + within xml tags. Capabilities in the parallel control flow will be triggered all + at once and will be executed parallely. Capabilities under parallel control flow needs to come within + xml tags. Capabilities in the recovery control flow will be triggered one after the other depending only on + precceding capability's failed execution. Capabilities under recovery control flow need to come within + xml tags. Upon the successful execution of this capability, another capability need to be executed for verifying + and starting the execution. It can be done via '' + xml command. A most basic plan with all compulsory runners is as follows, + + + + + + + + + + + " interface: services: "/prompt": diff --git a/std_capabilities2/interfaces/prompt/PromptPlanResponseRunner.yaml b/std_capabilities2/interfaces/prompt/PromptPlanResponseRunner.yaml index 010d05a..145879e 100644 --- a/std_capabilities2/interfaces/prompt/PromptPlanResponseRunner.yaml +++ b/std_capabilities2/interfaces/prompt/PromptPlanResponseRunner.yaml @@ -3,9 +3,11 @@ name: PromptPlanResponseRunner spec_version: 1.1 spec_type: interface -description: "The action can be trigged with an command such as ''. - This needs to be trigged at the end of each plan so that the plan created by the decision making authority such as an LLM can - be transferred to the capabilities2 executor for connection extraction and future triggering. " +description: "This capability depends on the prompt tools stack functionalities and allows an decision making authority such as an + LLM to verify new plan and start the execution at the end of the old execution plan. The capability can be trigged with + an command such as ''. This needs to be triggered + at the very end of each plan so that the plan created by the decision making authority such as an LLM can be transferred + to the capabilities2 executor for connection extraction and future triggering." interface: services: "/prompt": diff --git a/std_capabilities2/interfaces/prompt/PromptPoseRunner.yaml b/std_capabilities2/interfaces/prompt/PromptPoseRunner.yaml index ebec1e6..3b2c479 100644 --- a/std_capabilities2/interfaces/prompt/PromptPoseRunner.yaml +++ b/std_capabilities2/interfaces/prompt/PromptPoseRunner.yaml @@ -3,9 +3,12 @@ name: PromptPoseRunner spec_version: 1.1 spec_type: interface -description: "The capability can be trigged with an command such as '' - This needs to be trigged after an '' command so that data - extracted in the RobotPoseRunner can be prompted using PromptPoseRunner to decision making authority." +description: "This capability depends on the prompt tools stack functionalities and allows an decision making authority such as an + LLM to recieve robot pose and orientation data from the robot. This capability can be trigged with the xml command + ''. This will prompt the geometry_msgs/Pose msg in the xml + format and would allow the decision making authority to understand the position of the robot. Every execution plan + need to have this as one of the last compulsory capabilities to be executed, so that decision making authority is + updated about the latest position and orientation information of the robot which can then be used for future planning." interface: services: "/prompt": diff --git a/std_capabilities2/interfaces/prompt/PromptTextRunner.yaml b/std_capabilities2/interfaces/prompt/PromptTextRunner.yaml index eb0f53d..ca6e4ea 100644 --- a/std_capabilities2/interfaces/prompt/PromptTextRunner.yaml +++ b/std_capabilities2/interfaces/prompt/PromptTextRunner.yaml @@ -3,9 +3,10 @@ name: PromptTextRunner spec_version: 1.1 spec_type: interface -description: "The service can be trigged with an command such as '' - This needs to be trigged after an '' command so that data - extracted in the ListenerRunner can be prompted using PromptTextRunner to decision making authority." +description: "This capability depends on the prompt tools stack functionalities and allows an decision making authority such as an + LLM to recieve audio data as text from the robot. This capability can be trigged with the xml command + ''. This will prompt the decision making authority in the xml + format and would allow the decision making authority to understand the audio input the robot receives." interface: services: "/prompt": From 699f371ff4f3b73e92ddd16b7aaeea6bfb1d96d2 Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Fri, 8 Nov 2024 01:27:19 +1100 Subject: [PATCH 020/133] minor corrections and modifications to capabilities2_executor with launchfile, config file and default xl file addition --- capabilities2_executor/CMakeLists.txt | 9 ++- capabilities2_executor/config/.gitkeep | 0 capabilities2_executor/config/executor.yaml | 3 + .../capabilities_file_parser.hpp | 71 +++++++++---------- capabilities2_executor/launch/.gitkeep | 0 .../launch/capabilities2_executor.launch.py | 41 +++++++++++ capabilities2_executor/plans/default.xml | 11 +++ .../prompt_occupancy_runner.hpp | 6 +- .../prompt_plan_request_runner.hpp | 8 +-- .../prompt_pose_runner.hpp | 3 +- .../launch/capabilities2_server.launch.py | 8 +-- .../prompt/PromptPlanRequestRunner.yaml | 19 ++--- 12 files changed, 114 insertions(+), 65 deletions(-) delete mode 100644 capabilities2_executor/config/.gitkeep create mode 100644 capabilities2_executor/config/executor.yaml delete mode 100644 capabilities2_executor/launch/.gitkeep create mode 100644 capabilities2_executor/launch/capabilities2_executor.launch.py create mode 100644 capabilities2_executor/plans/default.xml diff --git a/capabilities2_executor/CMakeLists.txt b/capabilities2_executor/CMakeLists.txt index ccec815..16577b2 100644 --- a/capabilities2_executor/CMakeLists.txt +++ b/capabilities2_executor/CMakeLists.txt @@ -24,7 +24,7 @@ include_directories( ${TINYXML2_INCLUDE_DIRS} ) -add_library(${PROJECT_NAME} SHARED +add_executable(${PROJECT_NAME} src/capabilities_executor.cpp ) @@ -39,7 +39,7 @@ ament_target_dependencies(${PROJECT_NAME} TINYXML2 ) -add_library(${PROJECT_NAME}_file SHARED +add_executable(${PROJECT_NAME}_file src/capabilities_file_parser.cpp ) @@ -66,4 +66,9 @@ install(DIRECTORY config DESTINATION share/${PROJECT_NAME} ) +install(DIRECTORY plans + DESTINATION share/${PROJECT_NAME} +) + + ament_package() diff --git a/capabilities2_executor/config/.gitkeep b/capabilities2_executor/config/.gitkeep deleted file mode 100644 index e69de29..0000000 diff --git a/capabilities2_executor/config/executor.yaml b/capabilities2_executor/config/executor.yaml new file mode 100644 index 0000000..f4b0582 --- /dev/null +++ b/capabilities2_executor/config/executor.yaml @@ -0,0 +1,3 @@ +/**: + ros__parameters: + plan_file_path: "install/capabilities2_executor/share/capabilities2_executor/plans/default.xml" diff --git a/capabilities2_executor/include/capabilities2_executor/capabilities_file_parser.hpp b/capabilities2_executor/include/capabilities2_executor/capabilities_file_parser.hpp index c0263d9..36743e2 100644 --- a/capabilities2_executor/include/capabilities2_executor/capabilities_file_parser.hpp +++ b/capabilities2_executor/include/capabilities2_executor/capabilities_file_parser.hpp @@ -25,7 +25,7 @@ class CapabilitiesFileParser : public rclcpp::Node public: CapabilitiesFileParser(const rclcpp::NodeOptions& options = rclcpp::NodeOptions()) : Node("Capabilities2_File_Parser", options) { - declare_parameter("plan_file_path", "plan.xml"); + declare_parameter("plan_file_path", "install/capabilities2_executor/share/capabilities2_executor/plans/default.xml"); plan_file_path = get_parameter("plan_file_path").as_string(); this->client_ptr_ = rclcpp_action::create_client(this, "~/capabilities"); @@ -75,42 +75,41 @@ class CapabilitiesFileParser : public rclcpp::Node }; // result callback - send_goal_options.result_callback = - [this](const rclcpp_action::ClientGoalHandle::WrappedResult& result) { - switch (result.code) - { - case rclcpp_action::ResultCode::SUCCEEDED: - break; - case rclcpp_action::ResultCode::ABORTED: - RCLCPP_ERROR(this->get_logger(), "Goal was aborted"); - return; - case rclcpp_action::ResultCode::CANCELED: - RCLCPP_ERROR(this->get_logger(), "Goal was canceled"); - return; - default: - RCLCPP_ERROR(this->get_logger(), "Unknown result code"); - return; - } - - if (result.result->success) - { - RCLCPP_ERROR(this->get_logger(), "Plan executed successfully"); - } - else - { - RCLCPP_ERROR(this->get_logger(), "Plan failed to complete"); - - if (result.result->failed_elements.size() > 0) - { - RCLCPP_ERROR(this->get_logger(), "Plan failed due to incompatible XMLElements in the plan"); - - for (const auto& failed_element : result.result->failed_elements) - RCLCPP_ERROR(this->get_logger(), "Failed Elements : %s", failed_element.c_str()); - } - } + send_goal_options.result_callback = [this](const rclcpp_action::ClientGoalHandle::WrappedResult& result) { + switch (result.code) + { + case rclcpp_action::ResultCode::SUCCEEDED: + break; + case rclcpp_action::ResultCode::ABORTED: + RCLCPP_ERROR(this->get_logger(), "Goal was aborted"); + return; + case rclcpp_action::ResultCode::CANCELED: + RCLCPP_ERROR(this->get_logger(), "Goal was canceled"); + return; + default: + RCLCPP_ERROR(this->get_logger(), "Unknown result code"); + return; + } + + if (result.result->success) + { + RCLCPP_ERROR(this->get_logger(), "Plan executed successfully"); + } + else + { + RCLCPP_ERROR(this->get_logger(), "Plan failed to complete"); + + if (result.result->failed_elements.size() > 0) + { + RCLCPP_ERROR(this->get_logger(), "Plan failed due to incompatible XMLElements in the plan"); + + for (const auto& failed_element : result.result->failed_elements) + RCLCPP_ERROR(this->get_logger(), "Failed Elements : %s", failed_element.c_str()); + } + } - rclcpp::shutdown(); - }; + rclcpp::shutdown(); + }; this->client_ptr_->async_send_goal(goal_msg, send_goal_options); } diff --git a/capabilities2_executor/launch/.gitkeep b/capabilities2_executor/launch/.gitkeep deleted file mode 100644 index e69de29..0000000 diff --git a/capabilities2_executor/launch/capabilities2_executor.launch.py b/capabilities2_executor/launch/capabilities2_executor.launch.py new file mode 100644 index 0000000..c630bd5 --- /dev/null +++ b/capabilities2_executor/launch/capabilities2_executor.launch.py @@ -0,0 +1,41 @@ +''' +capabilities2_server launch file +''' + +import os +from launch import LaunchDescription +from launch_ros.actions import Node +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + """Generate launch description for capabilities2 server + + Returns: + LaunchDescription: The launch description for capabilities2 executor + """ + # load config file + executor_config = os.path.join(get_package_share_directory('capabilities2_executor'), 'config', 'executor.yaml') + + executor = Node( + package='capabilities2_executor', + namespace='', + executable='capabilities2_executor', + name='capabilities2_executor' + ), + + executor_file = Node( + package='capabilities2_executor', + namespace='', + executable='capabilities2_executor_file', + name='capabilities2_executor_file', + parameters=[executor_config] + ), + + # return + return LaunchDescription([ + executor, + executor_file + ]) diff --git a/capabilities2_executor/plans/default.xml b/capabilities2_executor/plans/default.xml new file mode 100644 index 0000000..cbcb513 --- /dev/null +++ b/capabilities2_executor/plans/default.xml @@ -0,0 +1,11 @@ + + + + + + + + + + + \ No newline at end of file diff --git a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_occupancy_runner.hpp b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_occupancy_runner.hpp index c35fc09..f3086f2 100644 --- a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_occupancy_runner.hpp +++ b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_occupancy_runner.hpp @@ -57,11 +57,9 @@ class PromptOccupancyRunner : public ServiceRunner prompt_msgs::srv::Prompt::Request request; - request.prompt.prompt = - "The OccupancyGrid of the robot shows the surrounding environment of the robot. The data is given as a ros2 // - nav_msgs occupancy_grid of which the content are " + data; + request.prompt.prompt = "The OccupancyGrid of the robot shows the surrounding environment of the robot. The data is given as a ros2 nav_msgs occupancy_grid of which the content are " + data; - prompt_msgs::msg::ModelOption modelOption1; + prompt_msgs::msg::ModelOption modelOption1; modelOption1.key = "model"; modelOption1.value = "llama3.1:8b"; diff --git a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_request_runner.hpp b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_request_runner.hpp index f42a945..c9c41c2 100644 --- a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_request_runner.hpp +++ b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_request_runner.hpp @@ -56,17 +56,13 @@ class PromptPlanRequestRunner : public ServiceRunner if (!replan) { - request.prompt.prompt = "Build a xml plan based on the availbale capabilities to acheive mentioned " // - "task. Return only the xml plan without explanations or comments"; + request.prompt.prompt = "Build a xml plan based on the availbale capabilities to acheive mentioned task. Return only the xml plan without explanations or comments"; } else { tinyxml2::XMLElement* failedElements = parameters->FirstChildElement("FailedElements"); - request.prompt.prompt = "Rebuild the xml plan based on the availbale capabilities to acheive mentioned " // - "task. Just give the xml plan without explanations or comments. These XML elements " // - "had incompatibilities. " + - std::string(failedElements->GetText()) + "Recorrect them as well"; + request.prompt.prompt = "Rebuild the xml plan based on the availbale capabilities to acheive mentioned task. Just give the xml plan without explanations or comments. These XML elements had incompatibilities. " + std::string(failedElements->GetText()) + "Recorrect them as well"; } prompt_msgs::msg::ModelOption modelOption1; diff --git a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_pose_runner.hpp b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_pose_runner.hpp index e26c2ed..09ba97b 100644 --- a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_pose_runner.hpp +++ b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_pose_runner.hpp @@ -57,8 +57,7 @@ class PromptPoseRunner : public ServiceRunner prompt_msgs::srv::Prompt::Request request; - request.prompt.prompt = - "The position of the robot is given as a standard ros2 geometry message of which the content are " + data; + request.prompt.prompt = "The position of the robot is given as a standard ros2 geometry message of which the content are " + data; prompt_msgs::msg::ModelOption modelOption1; modelOption1.key = "model"; diff --git a/capabilities2_server/launch/capabilities2_server.launch.py b/capabilities2_server/launch/capabilities2_server.launch.py index 759947b..ebe6b6a 100644 --- a/capabilities2_server/launch/capabilities2_server.launch.py +++ b/capabilities2_server/launch/capabilities2_server.launch.py @@ -17,11 +17,7 @@ def generate_launch_description(): LaunchDescription: The launch description for capabilities2 server """ # load config file - config = os.path.join( - get_package_share_directory('capabilities2_server'), - 'config', - 'capabilities.yaml' - ) + server_config = os.path.join(get_package_share_directory('capabilities2_server'), 'config', 'capabilities.yaml') # create bridge composition capabilities = ComposableNodeContainer( @@ -34,7 +30,7 @@ def generate_launch_description(): package='capabilities2_server', plugin='capabilities2_server::CapabilitiesServer', name='capabilities', - parameters=[config] + parameters=[server_config] ) ] ) diff --git a/std_capabilities2/interfaces/prompt/PromptPlanRequestRunner.yaml b/std_capabilities2/interfaces/prompt/PromptPlanRequestRunner.yaml index f2cba36..5299b0d 100644 --- a/std_capabilities2/interfaces/prompt/PromptPlanRequestRunner.yaml +++ b/std_capabilities2/interfaces/prompt/PromptPlanRequestRunner.yaml @@ -19,15 +19,16 @@ description: "This capability depends on the prompt tools stack functionalities xml tags. Upon the successful execution of this capability, another capability need to be executed for verifying and starting the execution. It can be done via '' xml command. A most basic plan with all compulsory runners is as follows, - - - - - - - - - + + + + + + + + + + " interface: From fb4adfb769bc3269af22889225c914c401a0a6e8 Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Sun, 10 Nov 2024 22:57:03 +1100 Subject: [PATCH 021/133] launch issue fixed on capabilities2_executor --- capabilities2_executor/CMakeLists.txt | 8 ++++++++ capabilities2_executor/config/executor.yaml | 3 ++- .../launch/capabilities2_executor.launch.py | 6 ++---- capabilities2_executor/plans/navigation_1.xml | 6 ++++++ 4 files changed, 18 insertions(+), 5 deletions(-) create mode 100644 capabilities2_executor/plans/navigation_1.xml diff --git a/capabilities2_executor/CMakeLists.txt b/capabilities2_executor/CMakeLists.txt index 16577b2..0300098 100644 --- a/capabilities2_executor/CMakeLists.txt +++ b/capabilities2_executor/CMakeLists.txt @@ -39,6 +39,10 @@ ament_target_dependencies(${PROJECT_NAME} TINYXML2 ) +install(TARGETS ${PROJECT_NAME} + DESTINATION lib/${PROJECT_NAME} +) + add_executable(${PROJECT_NAME}_file src/capabilities_file_parser.cpp ) @@ -54,6 +58,10 @@ ament_target_dependencies(${PROJECT_NAME}_file TINYXML2 ) +install(TARGETS ${PROJECT_NAME}_file + DESTINATION lib/${PROJECT_NAME} +) + install(DIRECTORY include/ DESTINATION include ) diff --git a/capabilities2_executor/config/executor.yaml b/capabilities2_executor/config/executor.yaml index f4b0582..07a0f29 100644 --- a/capabilities2_executor/config/executor.yaml +++ b/capabilities2_executor/config/executor.yaml @@ -1,3 +1,4 @@ /**: ros__parameters: - plan_file_path: "install/capabilities2_executor/share/capabilities2_executor/plans/default.xml" + plan_file_path: "install/capabilities2_executor/share/capabilities2_executor/plans/navigation_1.xml" + # plan_file_path: "install/capabilities2_executor/share/capabilities2_executor/plans/default.xml" diff --git a/capabilities2_executor/launch/capabilities2_executor.launch.py b/capabilities2_executor/launch/capabilities2_executor.launch.py index c630bd5..9be6c5d 100644 --- a/capabilities2_executor/launch/capabilities2_executor.launch.py +++ b/capabilities2_executor/launch/capabilities2_executor.launch.py @@ -5,8 +5,6 @@ import os from launch import LaunchDescription from launch_ros.actions import Node -from launch_ros.actions import ComposableNodeContainer -from launch_ros.descriptions import ComposableNode from ament_index_python.packages import get_package_share_directory @@ -24,7 +22,7 @@ def generate_launch_description(): namespace='', executable='capabilities2_executor', name='capabilities2_executor' - ), + ) executor_file = Node( package='capabilities2_executor', @@ -32,7 +30,7 @@ def generate_launch_description(): executable='capabilities2_executor_file', name='capabilities2_executor_file', parameters=[executor_config] - ), + ) # return return LaunchDescription([ diff --git a/capabilities2_executor/plans/navigation_1.xml b/capabilities2_executor/plans/navigation_1.xml new file mode 100644 index 0000000..e02793a --- /dev/null +++ b/capabilities2_executor/plans/navigation_1.xml @@ -0,0 +1,6 @@ + + + + + + \ No newline at end of file From 4311aaf088333eac0f80300e4f991063731d5cca Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Mon, 11 Nov 2024 16:17:25 +1100 Subject: [PATCH 022/133] moved capabilities2_fabric functionality to a seperate package with the same name(old capabilities_executor package) from capabilities_server package for clarity. removed a redundant ros2 action server as a result --- capabilities2_executor/readme.md | 5 - .../src/capabilities_executor.cpp | 9 - .../.clang-format | 0 .../CMakeLists.txt | 12 +- .../config/executor.yaml | 0 .../capabilities_fabric.hpp | 443 +++++++++++------- .../capabilities_file_parser.hpp | 6 +- .../capabilities_xml_parser.hpp | 0 .../structs/connection.hpp | 0 .../launch/capabilities2_executor.launch.py | 14 +- .../package.xml | 2 +- .../plans/default.xml | 0 .../plans/navigation_1.xml | 0 capabilities2_fabric/readme.md | 6 + .../src/capabilities_fabric.cpp | 9 + .../src/capabilities_file_parser.cpp | 0 capabilities2_msgs/CMakeLists.txt | 4 +- capabilities2_msgs/action/Connections.action | 14 - .../ConfigureCapability.srv} | 3 +- capabilities2_msgs/srv/TriggerCapability.srv | 3 + .../capabilities2_server/capabilities_api.hpp | 1 - .../capabilities_server.hpp | 158 ++----- 22 files changed, 367 insertions(+), 322 deletions(-) delete mode 100644 capabilities2_executor/readme.md delete mode 100644 capabilities2_executor/src/capabilities_executor.cpp rename {capabilities2_executor => capabilities2_fabric}/.clang-format (100%) rename {capabilities2_executor => capabilities2_fabric}/CMakeLists.txt (83%) rename {capabilities2_executor => capabilities2_fabric}/config/executor.yaml (100%) rename capabilities2_executor/include/capabilities2_executor/capabilities_executor.hpp => capabilities2_fabric/include/capabilities2_executor/capabilities_fabric.hpp (56%) rename {capabilities2_executor => capabilities2_fabric}/include/capabilities2_executor/capabilities_file_parser.hpp (94%) rename {capabilities2_executor => capabilities2_fabric}/include/capabilities2_executor/capabilities_xml_parser.hpp (100%) rename {capabilities2_executor => capabilities2_fabric}/include/capabilities2_executor/structs/connection.hpp (100%) rename {capabilities2_executor => capabilities2_fabric}/launch/capabilities2_executor.launch.py (68%) rename {capabilities2_executor => capabilities2_fabric}/package.xml (94%) rename {capabilities2_executor => capabilities2_fabric}/plans/default.xml (100%) rename {capabilities2_executor => capabilities2_fabric}/plans/navigation_1.xml (100%) create mode 100644 capabilities2_fabric/readme.md create mode 100644 capabilities2_fabric/src/capabilities_fabric.cpp rename {capabilities2_executor => capabilities2_fabric}/src/capabilities_file_parser.cpp (100%) delete mode 100644 capabilities2_msgs/action/Connections.action rename capabilities2_msgs/{msg/CapabilityConnection.msg => srv/ConfigureCapability.srv} (75%) create mode 100644 capabilities2_msgs/srv/TriggerCapability.srv diff --git a/capabilities2_executor/readme.md b/capabilities2_executor/readme.md deleted file mode 100644 index 999186c..0000000 --- a/capabilities2_executor/readme.md +++ /dev/null @@ -1,5 +0,0 @@ -# Capabilities2 Executor - -This is a simple executor that demonstrates how to use the capabilities2 library to execute multi-skill tasks. - -## Usage diff --git a/capabilities2_executor/src/capabilities_executor.cpp b/capabilities2_executor/src/capabilities_executor.cpp deleted file mode 100644 index 98c5a43..0000000 --- a/capabilities2_executor/src/capabilities_executor.cpp +++ /dev/null @@ -1,9 +0,0 @@ -#include - -int main(int argc, char * argv[]) -{ - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - return 0; -} diff --git a/capabilities2_executor/.clang-format b/capabilities2_fabric/.clang-format similarity index 100% rename from capabilities2_executor/.clang-format rename to capabilities2_fabric/.clang-format diff --git a/capabilities2_executor/CMakeLists.txt b/capabilities2_fabric/CMakeLists.txt similarity index 83% rename from capabilities2_executor/CMakeLists.txt rename to capabilities2_fabric/CMakeLists.txt index 0300098..5213f8a 100644 --- a/capabilities2_executor/CMakeLists.txt +++ b/capabilities2_fabric/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.8) -project(capabilities2_executor) +project(capabilities2_fabric) # Default to C++17 if(NOT CMAKE_CXX_STANDARD) @@ -25,7 +25,7 @@ include_directories( ) add_executable(${PROJECT_NAME} - src/capabilities_executor.cpp + src/capabilities_fabric.cpp ) target_link_libraries(${PROJECT_NAME} @@ -43,22 +43,22 @@ install(TARGETS ${PROJECT_NAME} DESTINATION lib/${PROJECT_NAME} ) -add_executable(${PROJECT_NAME}_file +add_executable(capabilities_file_parser src/capabilities_file_parser.cpp ) -target_link_libraries(${PROJECT_NAME}_file +target_link_libraries(capabilities_file_parser ${TINYXML2_LIBRARIES} ) -ament_target_dependencies(${PROJECT_NAME}_file +ament_target_dependencies(capabilities_file_parser rclcpp rclcpp_action capabilities2_msgs TINYXML2 ) -install(TARGETS ${PROJECT_NAME}_file +install(TARGETS capabilities_file_parser DESTINATION lib/${PROJECT_NAME} ) diff --git a/capabilities2_executor/config/executor.yaml b/capabilities2_fabric/config/executor.yaml similarity index 100% rename from capabilities2_executor/config/executor.yaml rename to capabilities2_fabric/config/executor.yaml diff --git a/capabilities2_executor/include/capabilities2_executor/capabilities_executor.hpp b/capabilities2_fabric/include/capabilities2_executor/capabilities_fabric.hpp similarity index 56% rename from capabilities2_executor/include/capabilities2_executor/capabilities_executor.hpp rename to capabilities2_fabric/include/capabilities2_executor/capabilities_fabric.hpp index dcc1340..bb368a4 100644 --- a/capabilities2_executor/include/capabilities2_executor/capabilities_executor.hpp +++ b/capabilities2_fabric/include/capabilities2_executor/capabilities_fabric.hpp @@ -10,15 +10,16 @@ #include -#include - #include -#include #include #include #include #include +#include +#include +#include +#include /** * @brief Capabilities Executor @@ -29,24 +30,26 @@ * */ -class CapabilitiesExecutor : public rclcpp::Node +class CapabilitiesFabric : public rclcpp::Node { public: - CapabilitiesExecutor(const rclcpp::NodeOptions& options = rclcpp::NodeOptions()) : Node("Capabilities2_Executor", options) + CapabilitiesFabric() : Node("Capabilities2_Executor") { control_tag_list = capabilities2_xml_parser::get_control_list(); this->planner_server_ = rclcpp_action::create_server( - this, "~/capabilities", std::bind(&CapabilitiesExecutor::handle_goal, this, std::placeholders::_1, std::placeholders::_2), - std::bind(&CapabilitiesExecutor::handle_cancel, this, std::placeholders::_1), - std::bind(&CapabilitiesExecutor::handle_accepted, this, std::placeholders::_1)); - - this->client_capabilities_ = rclcpp_action::create_client(this, "~/capabilities_fabric"); + this, "~/capabilities_fabric", std::bind(&CapabilitiesFabric::handle_goal, this, std::placeholders::_1, std::placeholders::_2), + std::bind(&CapabilitiesFabric::handle_cancel, this, std::placeholders::_1), + std::bind(&CapabilitiesFabric::handle_accepted, this, std::placeholders::_1)); get_interfaces_client_ = this->create_client("~/get_interfaces"); get_sem_interf_client_ = this->create_client("~/get_semantic_interfaces"); - + get_providers_client_ = this->create_client("~/get_providers"); establish_bond_client_ = this->create_client("~/establish_bond"); + use_capability_client_ = this->create_client("~/use_capability"); + free_capability_client_ = this->create_client("~/free_capability"); + trigger_capability_client_ = this->create_client("~/trigger_capability"); + configure_capability_client_ = this->create_client("~/configure_capability"); } private: @@ -97,7 +100,7 @@ class CapabilitiesExecutor : public rclcpp::Node */ void handle_accepted(const std::shared_ptr> goal_handle) { - execution_thread = std::make_unique(std::bind(&CapabilitiesExecutor::execute_plan, this, std::placeholders::_1), goal_handle); + fabric_thread = std::make_unique(std::bind(&CapabilitiesFabric::execute_plan, this, std::placeholders::_1), goal_handle); } /** @@ -281,6 +284,210 @@ class CapabilitiesExecutor : public rclcpp::Node return true; } + /** + * @brief Request use of capability from capabilities2 server + * + * @param capability capability name to be started + * @param provider provider of the capability + * @param bond_id bond_id for the capability + * + * @return `true` if use of capability is successful,`false` otherwise + */ + bool use_capability(const std::string& capability, const std::string& provider, const std::string& bond_id) + { + auto request_use = std::make_shared(); + + request_use->capability = capability; + request_use->preferred_provider = provider; + request_use->bond_id = bond_id; + + // send the request + auto result_future = use_capability_client_->async_send_request(request_use); + + RCLCPP_INFO(this->get_logger(), "Requesting to use %s capability from Capabilities2 Server", capability.c_str()); + + // wait for the result + if (rclcpp::spin_until_future_complete(shared_from_this(), result_future) != rclcpp::FutureReturnCode::SUCCESS) + { + RCLCPP_INFO(this->get_logger(), "Failed to use requested capability from Capabilities2 Server"); + return false; + } + + RCLCPP_INFO(this->get_logger(), "Successfully used requested capability from Capabilities2 Server"); + + return true; + } + + /** + * @brief Request use of capability from capabilities2 server + * + * @param capability capability name to be started + * @param bond_id bond_id for the capability + * + * @return `true` if use of capability is successful,`false` otherwise + */ + bool free_capability(const std::string& capability, const std::string& bond_id) + { + auto request_free = std::make_shared(); + + request_free->capability = capability; + request_free->bond_id = bond_id; + + // send the request + auto result_future = free_capability_client_->async_send_request(request_free); + + RCLCPP_INFO(this->get_logger(), "Requesting to free %s capability from Capabilities2 Server", capability.c_str()); + + // wait for the result + if (rclcpp::spin_until_future_complete(shared_from_this(), result_future) != rclcpp::FutureReturnCode::SUCCESS) + { + RCLCPP_INFO(this->get_logger(), "Failed to free requested capability from Capabilities2 Server"); + return false; + } + + RCLCPP_INFO(this->get_logger(), "Successfully freed requested capability from Capabilities2 Server"); + + return true; + } + + /** + * @brief Request use of capability from capabilities2 server + * + * @return `true` if configuration of capability is successful,`false` otherwise + */ + bool configure_capabilities() + { + for (const auto& [key, value] : connection_map) + { + auto request_configure = std::make_shared(); + + RCLCPP_INFO(this->get_logger(), "Configuring Node : %i", key); + + if (capabilities2_xml_parser::convert_to_string(value.source.parameters, request_configure->source.parameters)) + { + request_configure->source.capability = value.source.runner; + request_configure->source.provider = value.source.provider; + + RCLCPP_INFO(this->get_logger(), "Source Capability : %s", request_configure->source.capability.c_str()); + RCLCPP_INFO(this->get_logger(), "Source Provider : %s", request_configure->source.provider.c_str()); + RCLCPP_INFO(this->get_logger(), "Source Parameters : %s", request_configure->source.parameters.c_str()); + } + else + { + request_configure->source.capability = ""; + request_configure->source.provider = ""; + } + + if (capabilities2_xml_parser::convert_to_string(value.target_on_start.parameters, request_configure->target_on_start.parameters)) + { + request_configure->target_on_start.capability = value.target_on_start.runner; + request_configure->target_on_start.provider = value.target_on_start.provider; + + RCLCPP_INFO(this->get_logger(), "Triggered on start Capability : %s", request_configure->target_on_start.capability.c_str()); + RCLCPP_INFO(this->get_logger(), "Triggered on start Provider : %s", request_configure->target_on_start.provider.c_str()); + RCLCPP_INFO(this->get_logger(), "Triggered on start Parameters : %s", request_configure->target_on_start.parameters.c_str()); + } + else + { + request_configure->target_on_start.capability = ""; + request_configure->target_on_start.provider = ""; + } + + if (capabilities2_xml_parser::convert_to_string(value.target_on_stop.parameters, request_configure->target_on_stop.parameters)) + { + request_configure->target_on_stop.capability = value.target_on_stop.runner; + request_configure->target_on_stop.provider = value.target_on_stop.provider; + + RCLCPP_INFO(this->get_logger(), "Triggered on stop Capability : %s", request_configure->target_on_stop.capability.c_str()); + RCLCPP_INFO(this->get_logger(), "Triggered on stop Provider : %s", request_configure->target_on_stop.provider.c_str()); + RCLCPP_INFO(this->get_logger(), "Triggered on stop Parameters : %s", request_configure->target_on_stop.parameters.c_str()); + } + else + { + request_configure->target_on_stop.capability = ""; + request_configure->target_on_stop.provider = ""; + } + + if (capabilities2_xml_parser::convert_to_string(value.target_on_success.parameters, request_configure->target_on_success.parameters)) + { + request_configure->target_on_success.capability = value.target_on_success.runner; + request_configure->target_on_success.provider = value.target_on_success.provider; + + RCLCPP_INFO(this->get_logger(), "Triggered on success Capability : %s", request_configure->target_on_success.capability.c_str()); + RCLCPP_INFO(this->get_logger(), "Triggered on success Provider : %s", request_configure->target_on_success.provider.c_str()); + RCLCPP_INFO(this->get_logger(), "Triggered on success Parameters : %s", request_configure->target_on_success.parameters.c_str()); + } + else + { + request_configure->target_on_success.capability = ""; + request_configure->target_on_success.provider = ""; + } + + if (capabilities2_xml_parser::convert_to_string(value.target_on_failure.parameters, request_configure->target_on_failure.parameters)) + { + request_configure->target_on_failure.capability = value.target_on_failure.runner; + request_configure->target_on_failure.provider = value.target_on_failure.provider; + + RCLCPP_INFO(this->get_logger(), "Triggered on failure Capability : %s", request_configure->target_on_failure.capability.c_str()); + RCLCPP_INFO(this->get_logger(), "Triggered on failure Provider : %s", request_configure->target_on_failure.provider.c_str()); + RCLCPP_INFO(this->get_logger(), "Triggered on failure Parameters : %s", request_configure->target_on_failure.parameters.c_str()); + } + else + { + request_configure->target_on_failure.capability = ""; + request_configure->target_on_failure.provider = ""; + } + + // send the request + auto result_future = configure_capability_client_->async_send_request(request_configure); + + RCLCPP_INFO(this->get_logger(), "Configuring %s capability from Capabilities2 Server", request_configure->source.capability.c_str()); + + // wait for the result + if (rclcpp::spin_until_future_complete(shared_from_this(), result_future) != rclcpp::FutureReturnCode::SUCCESS) + { + RCLCPP_INFO(this->get_logger(), "Failed to configure requested capability from Capabilities2 Server"); + return false; + } + + RCLCPP_INFO(this->get_logger(), "Successfully configured requested capability from Capabilities2 Server"); + } + + return true; + } + + /** + * @brief Trigger the first node + * + * @return `true` if triggering is successful,`false` otherwise + */ + bool trigger_first_node() + { + auto request_trigger = std::make_shared(); + + std::string parameter_string; + capabilities2_xml_parser::convert_to_string(connection_map[0].source.parameters, parameter_string); + + request_trigger->capability = connection_map[0].source.runner; + request_trigger->parameters = parameter_string; + + // send the request + auto result_future = trigger_capability_client_->async_send_request(request_trigger); + + RCLCPP_INFO(this->get_logger(), "Requesting to trigger %s capability from Capabilities2 Server", connection_map[0].source.runner.c_str()); + + // wait for the result + if (rclcpp::spin_until_future_complete(shared_from_this(), result_future) != rclcpp::FutureReturnCode::SUCCESS) + { + RCLCPP_INFO(this->get_logger(), "Failed to trigger requested capability from Capabilities2 Server"); + return false; + } + + RCLCPP_INFO(this->get_logger(), "Successfully triggered requested capability from Capabilities2 Server"); + + return true; + } + /** * @brief execute the plan * @@ -290,6 +497,8 @@ class CapabilitiesExecutor : public rclcpp::Node { auto result = std::make_shared(); + RCLCPP_INFO(this->get_logger(), "Execution started"); + // verify the plan if (!request_information()) { @@ -302,6 +511,8 @@ class CapabilitiesExecutor : public rclcpp::Node RCLCPP_INFO(this->get_logger(), "Server Execution Cancelled"); } + RCLCPP_INFO(this->get_logger(), "Interface retreival successful"); + // verify the plan if (!verify_plan()) { @@ -331,12 +542,18 @@ class CapabilitiesExecutor : public rclcpp::Node RCLCPP_INFO(this->get_logger(), "Server Execution Cancelled"); } + RCLCPP_INFO(this->get_logger(), "Plan verification successful"); + // extract the plan from the XMLDocument tinyxml2::XMLElement* plan = capabilities2_xml_parser::get_plan(document); + RCLCPP_INFO(this->get_logger(), "Plan conversion successful"); + // Extract the connections from the plan capabilities2_xml_parser::extract_connections(plan, connection_map); + RCLCPP_INFO(this->get_logger(), "Connection extraction successful"); + // estasblish the bond with the server if (!establish_bond()) { @@ -349,171 +566,72 @@ class CapabilitiesExecutor : public rclcpp::Node RCLCPP_INFO(this->get_logger(), "Server Execution Cancelled"); } - auto connection_goal_msg = capabilities2_msgs::action::Connections::Goal(); - connection_goal_msg.bond_id = bond_id; - connection_goal_msg.header.stamp = this->get_clock()->now(); - - capabilities2_msgs::msg::CapabilityConnection connection_msg; + RCLCPP_INFO(this->get_logger(), "Bond establishment successful"); + // start all runners and interfaces that the connections depend on for (const auto& [key, value] : connection_map) { - RCLCPP_INFO(this->get_logger(), "Node : %i", key); + RCLCPP_INFO(this->get_logger(), "Starting capability of Node : %i", key); - if (capabilities2_xml_parser::convert_to_string(value.source.parameters, connection_msg.source.parameters)) + // start the capability with bond id. + if (use_capability(value.source.runner, value.source.provider, bond_id)) { - connection_msg.source.capability = value.source.runner; - connection_msg.source.provider = value.source.provider; - - RCLCPP_INFO(this->get_logger(), "Source Capability : %s", connection_msg.source.capability.c_str()); - RCLCPP_INFO(this->get_logger(), "Source Provider : %s", connection_msg.source.provider.c_str()); - RCLCPP_INFO(this->get_logger(), "Source Parameters : %s", connection_msg.source.parameters.c_str()); + // capability started succeesfully + RCLCPP_INFO(get_logger(), "Capability started: %s", value.source.runner.c_str()); } else { - connection_msg.source.capability = ""; - connection_msg.source.provider = ""; - } - - if (capabilities2_xml_parser::convert_to_string(value.target_on_start.parameters, connection_msg.target_on_start.parameters)) - { - connection_msg.target_on_start.capability = value.target_on_start.runner; - connection_msg.target_on_start.provider = value.target_on_start.provider; + std::string parameter_string; + capabilities2_xml_parser::convert_to_string(value.source.parameters, parameter_string); - RCLCPP_INFO(this->get_logger(), "Triggered on start Capability : %s", connection_msg.target_on_start.capability.c_str()); - RCLCPP_INFO(this->get_logger(), "Triggered on start Provider : %s", connection_msg.target_on_start.provider.c_str()); - RCLCPP_INFO(this->get_logger(), "Triggered on start Parameters : %s", connection_msg.target_on_start.parameters.c_str()); - } - else - { - connection_msg.target_on_start.capability = ""; - connection_msg.target_on_start.provider = ""; - } - - if (capabilities2_xml_parser::convert_to_string(value.target_on_stop.parameters, connection_msg.target_on_stop.parameters)) - { - connection_msg.target_on_stop.capability = value.target_on_stop.runner; - connection_msg.target_on_stop.provider = value.target_on_stop.provider; - - RCLCPP_INFO(this->get_logger(), "Triggered on stop Capability : %s", connection_msg.target_on_stop.capability.c_str()); - RCLCPP_INFO(this->get_logger(), "Triggered on stop Provider : %s", connection_msg.target_on_stop.provider.c_str()); - RCLCPP_INFO(this->get_logger(), "Triggered on stop Parameters : %s", connection_msg.target_on_stop.parameters.c_str()); - } - else - { - connection_msg.target_on_stop.capability = ""; - connection_msg.target_on_stop.provider = ""; + // capability failed. so add it to failed connections to be sent to the action client + result->failed_elements.push_back(parameter_string); + RCLCPP_ERROR(get_logger(), "Capability failed: %s", parameter_string.c_str()); } + } - if (capabilities2_xml_parser::convert_to_string(value.target_on_success.parameters, connection_msg.target_on_success.parameters)) - { - connection_msg.target_on_success.capability = value.target_on_success.runner; - connection_msg.target_on_success.provider = value.target_on_success.provider; + RCLCPP_INFO(this->get_logger(), "All capability starting successful"); - RCLCPP_INFO(this->get_logger(), "Triggered on success Capability : %s", connection_msg.target_on_success.capability.c_str()); - RCLCPP_INFO(this->get_logger(), "Triggered on success Provider : %s", connection_msg.target_on_success.provider.c_str()); - RCLCPP_INFO(this->get_logger(), "Triggered on success Parameters : %s", connection_msg.target_on_success.parameters.c_str()); - } - else - { - connection_msg.target_on_success.capability = ""; - connection_msg.target_on_success.provider = ""; - } - - if (capabilities2_xml_parser::convert_to_string(value.target_on_failure.parameters, connection_msg.target_on_failure.parameters)) - { - connection_msg.target_on_failure.capability = value.target_on_failure.runner; - connection_msg.target_on_failure.provider = value.target_on_failure.provider; + // check if there are any failed elements of the plan + if (result->failed_elements.size() > 0) + { + // there are failed connections. so cancel the action server process. and let the action + // client know that about the failed connection + server_goal_handle->canceled(result); - RCLCPP_INFO(this->get_logger(), "Triggered on failure Capability : %s", connection_msg.target_on_failure.capability.c_str()); - RCLCPP_INFO(this->get_logger(), "Triggered on failure Provider : %s", connection_msg.target_on_failure.provider.c_str()); - RCLCPP_INFO(this->get_logger(), "Triggered on failure Parameters : %s", connection_msg.target_on_failure.parameters.c_str()); - } - else + // free the capabilites that were started since action execution failed + for (const auto& [key, value] : connection_map) { - connection_msg.target_on_failure.capability = ""; - connection_msg.target_on_failure.provider = ""; + free_capability(value.source.runner, bond_id); + RCLCPP_ERROR(get_logger(), "Capability freed due to failure: %s", value.source.runner.c_str()); } - - connection_goal_msg.connections.push_back(connection_msg); } - auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); - - // send goal options - // goal response callback - send_goal_options.goal_response_callback = - [this, server_goal_handle](const rclcpp_action::ClientGoalHandle::SharedPtr& goal_handle) { - if (!goal_handle) - { - RCLCPP_ERROR(this->get_logger(), "Goal was rejected by server"); - - auto result = std::make_shared(); - - // TODO: improve with error codes - result->success = false; - server_goal_handle->canceled(result); - - RCLCPP_INFO(this->get_logger(), "Server Execution Cancelled"); - } - else - { - RCLCPP_INFO(this->get_logger(), "Goal accepted by server, waiting for result"); - } - }; - - // result callback - send_goal_options.result_callback = - [this, server_goal_handle](const rclcpp_action::ClientGoalHandle::WrappedResult& result) { - auto result_out = std::make_shared(); - - switch (result.code) - { - case rclcpp_action::ResultCode::SUCCEEDED: - break; - case rclcpp_action::ResultCode::ABORTED: - RCLCPP_ERROR(this->get_logger(), "Goal was aborted"); - - // TODO: improve with error codes - result_out->success = false; - server_goal_handle->canceled(result_out); - - RCLCPP_INFO(this->get_logger(), "Server Execution Cancelled"); - - return; - case rclcpp_action::ResultCode::CANCELED: - RCLCPP_ERROR(this->get_logger(), "Goal was canceled"); - - // TODO: improve with error codes - result_out->success = false; - server_goal_handle->canceled(result_out); - - RCLCPP_INFO(this->get_logger(), "Server Execution Cancelled"); - - return; - default: - RCLCPP_ERROR(this->get_logger(), "Unknown result code"); + // configuring the capabilities + if (!configure_capabilities()) + { + RCLCPP_INFO(this->get_logger(), "Configuring capabilities failed"); - // TODO: improve with error codes - result_out->success = false; - server_goal_handle->canceled(result_out); + // TODO: improve with error codes + result->success = false; + server_goal_handle->canceled(result); - RCLCPP_INFO(this->get_logger(), "Server Execution Cancelled"); - return; - } + RCLCPP_INFO(this->get_logger(), "Server Execution Cancelled"); + } - if (result.result->failed_connections.size() == 0) - { - // TODO: improve with error codes - result_out->success = true; - server_goal_handle->succeed(result_out); + RCLCPP_INFO(this->get_logger(), "Capability configuration successful"); - RCLCPP_INFO(this->get_logger(), "Server Execution Succeeded"); - } + // triggering the first node to start the fabric + if (!trigger_first_node()) + { + RCLCPP_INFO(this->get_logger(), "Triggering first capability failed"); - rclcpp::shutdown(); - }; + // TODO: improve with error codes + result->success = false; + server_goal_handle->canceled(result); - this->client_capabilities_->async_send_goal(connection_goal_msg, send_goal_options); + RCLCPP_INFO(this->get_logger(), "Server Execution Cancelled"); + } } private: @@ -533,7 +651,7 @@ class CapabilitiesExecutor : public rclcpp::Node std::map connection_map; /** Execution Thread */ - std::shared_ptr execution_thread; + std::shared_ptr fabric_thread; /** Interface List */ std::vector interface_list; @@ -547,9 +665,6 @@ class CapabilitiesExecutor : public rclcpp::Node /** Invalid events list */ std::vector rejected_list; - /** action client for connecting with capabilities server*/ - rclcpp_action::Client::SharedPtr client_capabilities_; - /** action server that exposes executor*/ std::shared_ptr> planner_server_; @@ -567,4 +682,16 @@ class CapabilitiesExecutor : public rclcpp::Node /** establish bond */ rclcpp::Client::SharedPtr establish_bond_client_; + + /** use an selected capability */ + rclcpp::Client::SharedPtr use_capability_client_; + + /** free an selected capability */ + rclcpp::Client::SharedPtr free_capability_client_; + + /** configure an selected capability */ + rclcpp::Client::SharedPtr configure_capability_client_; + + /** trigger an selected capability */ + rclcpp::Client::SharedPtr trigger_capability_client_; }; \ No newline at end of file diff --git a/capabilities2_executor/include/capabilities2_executor/capabilities_file_parser.hpp b/capabilities2_fabric/include/capabilities2_executor/capabilities_file_parser.hpp similarity index 94% rename from capabilities2_executor/include/capabilities2_executor/capabilities_file_parser.hpp rename to capabilities2_fabric/include/capabilities2_executor/capabilities_file_parser.hpp index 36743e2..4460047 100644 --- a/capabilities2_executor/include/capabilities2_executor/capabilities_file_parser.hpp +++ b/capabilities2_fabric/include/capabilities2_executor/capabilities_file_parser.hpp @@ -23,12 +23,12 @@ class CapabilitiesFileParser : public rclcpp::Node { public: - CapabilitiesFileParser(const rclcpp::NodeOptions& options = rclcpp::NodeOptions()) : Node("Capabilities2_File_Parser", options) + CapabilitiesFileParser() : Node("Capabilities2_File_Parser") { declare_parameter("plan_file_path", "install/capabilities2_executor/share/capabilities2_executor/plans/default.xml"); plan_file_path = get_parameter("plan_file_path").as_string(); - this->client_ptr_ = rclcpp_action::create_client(this, "~/capabilities"); + this->client_ptr_ = rclcpp_action::create_client(this, "~/capabilities_fabric"); this->timer_ = this->create_wall_timer(std::chrono::milliseconds(500), std::bind(&CapabilitiesFileParser::send_goal, this)); } @@ -47,6 +47,8 @@ class CapabilitiesFileParser : public rclcpp::Node rclcpp::shutdown(); } + RCLCPP_INFO(this->get_logger(), "Sucessfully loaded the file from path : %s", plan_file_path.c_str()); + if (!this->client_ptr_->wait_for_action_server()) { RCLCPP_ERROR(this->get_logger(), "Action server not available after waiting"); diff --git a/capabilities2_executor/include/capabilities2_executor/capabilities_xml_parser.hpp b/capabilities2_fabric/include/capabilities2_executor/capabilities_xml_parser.hpp similarity index 100% rename from capabilities2_executor/include/capabilities2_executor/capabilities_xml_parser.hpp rename to capabilities2_fabric/include/capabilities2_executor/capabilities_xml_parser.hpp diff --git a/capabilities2_executor/include/capabilities2_executor/structs/connection.hpp b/capabilities2_fabric/include/capabilities2_executor/structs/connection.hpp similarity index 100% rename from capabilities2_executor/include/capabilities2_executor/structs/connection.hpp rename to capabilities2_fabric/include/capabilities2_executor/structs/connection.hpp diff --git a/capabilities2_executor/launch/capabilities2_executor.launch.py b/capabilities2_fabric/launch/capabilities2_executor.launch.py similarity index 68% rename from capabilities2_executor/launch/capabilities2_executor.launch.py rename to capabilities2_fabric/launch/capabilities2_executor.launch.py index 9be6c5d..d3a4ccc 100644 --- a/capabilities2_executor/launch/capabilities2_executor.launch.py +++ b/capabilities2_fabric/launch/capabilities2_executor.launch.py @@ -15,20 +15,20 @@ def generate_launch_description(): LaunchDescription: The launch description for capabilities2 executor """ # load config file - executor_config = os.path.join(get_package_share_directory('capabilities2_executor'), 'config', 'executor.yaml') + executor_config = os.path.join(get_package_share_directory('capabilities2_fabric'), 'config', 'executor.yaml') executor = Node( - package='capabilities2_executor', + package='capabilities2_fabric', namespace='', - executable='capabilities2_executor', - name='capabilities2_executor' + executable='capabilities2_fabric', + name='capabilities2_fabric' ) executor_file = Node( - package='capabilities2_executor', + package='capabilities2_fabric', namespace='', - executable='capabilities2_executor_file', - name='capabilities2_executor_file', + executable='capabilities2_file_parser', + name='capabilities2_file_parser', parameters=[executor_config] ) diff --git a/capabilities2_executor/package.xml b/capabilities2_fabric/package.xml similarity index 94% rename from capabilities2_executor/package.xml rename to capabilities2_fabric/package.xml index d8d8132..25f55e8 100644 --- a/capabilities2_executor/package.xml +++ b/capabilities2_fabric/package.xml @@ -1,7 +1,7 @@ - capabilities2_executor + capabilities2_fabric 0.0.0 TODO: Package description kalana diff --git a/capabilities2_executor/plans/default.xml b/capabilities2_fabric/plans/default.xml similarity index 100% rename from capabilities2_executor/plans/default.xml rename to capabilities2_fabric/plans/default.xml diff --git a/capabilities2_executor/plans/navigation_1.xml b/capabilities2_fabric/plans/navigation_1.xml similarity index 100% rename from capabilities2_executor/plans/navigation_1.xml rename to capabilities2_fabric/plans/navigation_1.xml diff --git a/capabilities2_fabric/readme.md b/capabilities2_fabric/readme.md new file mode 100644 index 0000000..b024286 --- /dev/null +++ b/capabilities2_fabric/readme.md @@ -0,0 +1,6 @@ +# Capabilities2 Fabric + +This is a executor that extracts the event relationship from the provided xml plan and transfers that to the capabilities2 server + +## Usage + diff --git a/capabilities2_fabric/src/capabilities_fabric.cpp b/capabilities2_fabric/src/capabilities_fabric.cpp new file mode 100644 index 0000000..19e8bc9 --- /dev/null +++ b/capabilities2_fabric/src/capabilities_fabric.cpp @@ -0,0 +1,9 @@ +#include + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/capabilities2_executor/src/capabilities_file_parser.cpp b/capabilities2_fabric/src/capabilities_file_parser.cpp similarity index 100% rename from capabilities2_executor/src/capabilities_file_parser.cpp rename to capabilities2_fabric/src/capabilities_file_parser.cpp diff --git a/capabilities2_msgs/CMakeLists.txt b/capabilities2_msgs/CMakeLists.txt index 669eb9c..5ea912d 100644 --- a/capabilities2_msgs/CMakeLists.txt +++ b/capabilities2_msgs/CMakeLists.txt @@ -24,7 +24,6 @@ set(msg_files "msg/NaturalCapability.msg" "msg/CapabilityCommand.msg" "msg/CapabilityResponse.msg" - "msg/CapabilityConnection.msg" ) set(srv_files @@ -41,13 +40,14 @@ set(srv_files "srv/StartCapability.srv" "srv/StopCapability.srv" "srv/UseCapability.srv" + "srv/ConfigureCapability.srv" + "srv/TriggerCapability.srv" ) set(action_files "action/Capability.action" "action/Launch.action" "action/Plan.action" - "action/Connections.action" ) rosidl_generate_interfaces(${PROJECT_NAME} diff --git a/capabilities2_msgs/action/Connections.action b/capabilities2_msgs/action/Connections.action deleted file mode 100644 index 80c885d..0000000 --- a/capabilities2_msgs/action/Connections.action +++ /dev/null @@ -1,14 +0,0 @@ -# action to send a list of interface connections to capabilities server -# goal -std_msgs/Header header -string bond_id -CapabilityConnection[] connections ---- -# result -std_msgs/Header header -CapabilityConnection[] failed_connections ---- -# feedback -std_msgs/Header header -int32 success_count -int32 total_count diff --git a/capabilities2_msgs/msg/CapabilityConnection.msg b/capabilities2_msgs/srv/ConfigureCapability.srv similarity index 75% rename from capabilities2_msgs/msg/CapabilityConnection.msg rename to capabilities2_msgs/srv/ConfigureCapability.srv index b350b5c..66cf00c 100644 --- a/capabilities2_msgs/msg/CapabilityConnection.msg +++ b/capabilities2_msgs/srv/ConfigureCapability.srv @@ -2,4 +2,5 @@ Capability source Capability target_on_start Capability target_on_stop Capability target_on_success -Capability target_on_failure \ No newline at end of file +Capability target_on_failure +--- \ No newline at end of file diff --git a/capabilities2_msgs/srv/TriggerCapability.srv b/capabilities2_msgs/srv/TriggerCapability.srv new file mode 100644 index 0000000..b9160a7 --- /dev/null +++ b/capabilities2_msgs/srv/TriggerCapability.srv @@ -0,0 +1,3 @@ +string capability +string parameters +--- diff --git a/capabilities2_server/include/capabilities2_server/capabilities_api.hpp b/capabilities2_server/include/capabilities2_server/capabilities_api.hpp index 6a9a805..04c6c88 100644 --- a/capabilities2_server/include/capabilities2_server/capabilities_api.hpp +++ b/capabilities2_server/include/capabilities2_server/capabilities_api.hpp @@ -18,7 +18,6 @@ #include #include -#include #include #include #include diff --git a/capabilities2_server/include/capabilities2_server/capabilities_server.hpp b/capabilities2_server/include/capabilities2_server/capabilities_server.hpp index 311209f..d6a7559 100644 --- a/capabilities2_server/include/capabilities2_server/capabilities_server.hpp +++ b/capabilities2_server/include/capabilities2_server/capabilities_server.hpp @@ -16,14 +16,17 @@ #include #include + #include #include -#include + #include #include #include #include +#include #include +#include #include #include #include @@ -32,7 +35,6 @@ #include #include #include -#include namespace capabilities2_server { @@ -134,11 +136,21 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI "~/free_capability", std::bind(&CapabilitiesServer::free_capability_cb, this, std::placeholders::_1, std::placeholders::_2)); + // trigger capability + trigger_capability_srv_ = create_service( + "~/trigger_capability", + std::bind(&CapabilitiesServer::trigger_capability_cb, this, std::placeholders::_1, std::placeholders::_2)); + // use capability use_capability_srv_ = create_service( "~/use_capability", std::bind(&CapabilitiesServer::use_capability_cb, this, std::placeholders::_1, std::placeholders::_2)); + // configure capability + configure_capability_srv_ = create_service( + "~/configure_capability", + std::bind(&CapabilitiesServer::configure_capability_cb, this, std::placeholders::_1, std::placeholders::_2)); + // register capability register_capability_srv_ = create_service( "~/register_capability", @@ -173,12 +185,6 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI "~/get_running_capabilities", std::bind(&CapabilitiesServer::get_running_capabilities_cb, this, std::placeholders::_1, std::placeholders::_2)); - this->capabilities_fabric_server = rclcpp_action::create_server( - this, "~/capabilities_fabric", - std::bind(&CapabilitiesServer::handle_goal, this, std::placeholders::_1, std::placeholders::_2), - std::bind(&CapabilitiesServer::handle_cancel, this, std::placeholders::_1), - std::bind(&CapabilitiesServer::handle_accepted, this, std::placeholders::_1)); - // log ready RCLCPP_INFO(get_logger(), "capabilities server started"); @@ -221,6 +227,17 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI res->successful = true; } + // trigger capability + void trigger_capability_cb(const std::shared_ptr req, + std::shared_ptr res) + { + // try stopping capability + // TODO: handle errors + trigger_capability(req->capability, req->parameters); + + // response is empty + } + // free capability void free_capability_cb(const std::shared_ptr req, std::shared_ptr res) @@ -273,6 +290,19 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI // response is empty } + // use capability + void configure_capability_cb(const std::shared_ptr req, + std::shared_ptr res) + { + // setup triggers between parameters + set_triggers(req->source.capability, req->target_on_start.capability, req->target_on_start.parameters, + req->target_on_failure.capability, req->target_on_failure.parameters, + req->target_on_success.capability, req->target_on_success.parameters, req->target_on_stop.capability, + req->target_on_stop.parameters); + + // response is empty + } + // register capability void register_capability_cb(const std::shared_ptr req, std::shared_ptr res) @@ -370,112 +400,7 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI res->running_capabilities = get_running_capabilities(); } - /** - * @brief Handle the goal request that comes in from client. returns whether goal is accepted or rejected - * - * @param uuid uuid of the goal - * @param goal pointer to the action goal message with connections - * @return rclcpp_action::GoalResponse - */ - rclcpp_action::GoalResponse handle_goal(const rclcpp_action::GoalUUID& uuid, - std::shared_ptr goal) - { - RCLCPP_INFO(this->get_logger(), "Received the goal request with connections"); - (void)uuid; - - return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; - } - - /** - * @brief Handle the goal cancel request that comes in from client. - * - * @param goal_handle pointer to the action goal handle - * @return rclcpp_action::GoalResponse - */ - rclcpp_action::CancelResponse handle_cancel( - const std::shared_ptr> goal_handle) - { - RCLCPP_INFO(this->get_logger(), "Received the request to cancel the plan"); - (void)goal_handle; - - return rclcpp_action::CancelResponse::ACCEPT; - } - - /** - * @brief Handle the goal accept event originating from handle_goal. - * - * @param goal_handle pointer to the action goal handle - */ - void handle_accepted( - const std::shared_ptr> goal_handle) - { - fabric_launch_thread = std::make_unique( - std::bind(&CapabilitiesServer::launch_fabric, this, std::placeholders::_1), goal_handle); - } - private: - /** - * @brief Load capabilities from a package path - * - * @param package_path - */ - void launch_fabric( - const std::shared_ptr> goal_handle) - { - auto goal = goal_handle->get_goal(); - auto result = std::make_shared(); - - // start all runners and interfaces that the connections depend on - for (const auto& connection : goal->connections) - { - // start the capability with bond id. - if (use_capability(shared_from_this(), connection.source.capability, connection.source.provider, goal->bond_id)) - { - // capability started succeesfully - RCLCPP_INFO(get_logger(), "Capability started: %s", connection.source.capability.c_str()); - } - else - { - //capability failed. so add it to failed connections to be sent to the action client - result->failed_connections.push_back(connection); - RCLCPP_ERROR(get_logger(), "Capability failed: %s", connection.source.capability.c_str()); - } - } - - // check if there are any failed connections - if (result->failed_connections.size() > 0) - { - // there are failed connections. so cancel the action server process. and let the action - // client know that about the failed connection - goal_handle->canceled(result); - - // free the capabilites that were started since action execution failed - for (const auto& connection : goal->connections) - { - free_capability(connection.source.capability, goal->bond_id); - RCLCPP_ERROR(get_logger(), "Capability freed due to failure: %s", connection.source.capability.c_str()); - } - } - - // establish relationships for all the connections received. - for (const auto& connection : goal->connections) - { - set_triggers(connection.source.capability, connection.target_on_start.capability, - connection.target_on_start.parameters, connection.target_on_failure.capability, - connection.target_on_failure.parameters, connection.target_on_success.capability, - connection.target_on_success.parameters, connection.target_on_stop.capability, - connection.target_on_stop.parameters); - } - - // identify the first capability in the fabric - auto first_node = goal->connections[0]; - - // trigger the first node so fabric starts rippling - trigger_capability(first_node.source.capability, first_node.source.parameters); - - // return success and let the action client know that the fabric was launched. - goal_handle->succeed(result); - } void load_capabilities(const std::string& package_path) { @@ -641,6 +566,10 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI rclcpp::Service::SharedPtr stop_capability_srv_; // free capability rclcpp::Service::SharedPtr free_capability_srv_; + // free capability + rclcpp::Service::SharedPtr configure_capability_srv_; + // free capability + rclcpp::Service::SharedPtr trigger_capability_srv_; // use capability rclcpp::Service::SharedPtr use_capability_srv_; // register capability @@ -659,9 +588,6 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI rclcpp::Service::SharedPtr get_remappings_srv_; // get running capabilities rclcpp::Service::SharedPtr get_running_capabilities_srv_; - - /** action server that exposes cabapilities fabric*/ - std::shared_ptr> capabilities_fabric_server; }; } // namespace capabilities2_server From 00a832a52a49a4028a56bd3640ad4cbb2931dba3 Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Mon, 11 Nov 2024 20:59:05 +1100 Subject: [PATCH 023/133] improved documentation --- ...s2_executor.launch.py => fabric.launch.py} | 0 capabilities2_server/docs/register.md | 22 ++++ capabilities2_server/docs/terminal_usage.md | 26 +++++ ...ies2_server.launch.py => server.launch.py} | 0 capabilities2_server/readme.md | 100 ++++------------ docs/design.md | 15 +++ docs/run_test_scripts.md | 22 +++- docs/setup.md | 61 ++++++++++ docs/setup_with_dev.md | 84 +++++++++++++ docs/testing.md | 5 + readme.md | 110 ++++-------------- 11 files changed, 275 insertions(+), 170 deletions(-) rename capabilities2_fabric/launch/{capabilities2_executor.launch.py => fabric.launch.py} (100%) create mode 100644 capabilities2_server/docs/register.md create mode 100644 capabilities2_server/docs/terminal_usage.md rename capabilities2_server/launch/{capabilities2_server.launch.py => server.launch.py} (100%) create mode 100644 docs/design.md create mode 100644 docs/setup.md create mode 100644 docs/setup_with_dev.md create mode 100644 docs/testing.md diff --git a/capabilities2_fabric/launch/capabilities2_executor.launch.py b/capabilities2_fabric/launch/fabric.launch.py similarity index 100% rename from capabilities2_fabric/launch/capabilities2_executor.launch.py rename to capabilities2_fabric/launch/fabric.launch.py diff --git a/capabilities2_server/docs/register.md b/capabilities2_server/docs/register.md new file mode 100644 index 0000000..b5e9578 --- /dev/null +++ b/capabilities2_server/docs/register.md @@ -0,0 +1,22 @@ +## Registering a capability + +Capabilities can be registered by exporting them in a package. The capabilities2 server will read the capabilities from the package and make them available to the user. Basic capability use case needs two components to be registered; `capability_interface` and `capability_provider`. Optionally, a third component can be added as `semantic_capability_interface`. + +```xml + + + + interfaces/cool_cap.yaml + + + + providers/cool_cap.yaml + + + + semantic_interfaces/not_cool_cap.yaml + + +``` + +Capabilities can also be registered through a service call. This is useful for registering capabilities that are not exported in a package. The service call has been implemented as a node in the capabilities2 package. Use the node to register capabilities during runtime. This method can also be used to update capabilities. diff --git a/capabilities2_server/docs/terminal_usage.md b/capabilities2_server/docs/terminal_usage.md new file mode 100644 index 0000000..20751db --- /dev/null +++ b/capabilities2_server/docs/terminal_usage.md @@ -0,0 +1,26 @@ +## Terminal based capability usage + +Using a capability requires the user to establish a bond with the capabilities2 server. The bond is used to maintain a persistent connection with the server. + +first, inspect the available capabilities provided under this server on this robot. + +```bash +ros2 service call /capabilities/get_capability_specs capabilities2_msgs/srv/GetCapabilitySpecs +``` + +then, request a bond id to become a persistent user + +```bash +ros2 service call /capabilities/establish_bond capabilities2_msgs/srv/EstablishBond + +# this bond needs to be updated every second by publishing a heartbeat the bond topic. Replace the ... with value received from above call +ros2 topic pub /capabilities/bonds ... +``` + +then request to use a capability + +```bash +ros2 service call /capabilities/use_capability capabilities2_msgs/srv/UseCapability +``` + +This capability can be freed by calling the `free_capability` service, or just let the bond expire. The capability will be freed automatically. diff --git a/capabilities2_server/launch/capabilities2_server.launch.py b/capabilities2_server/launch/server.launch.py similarity index 100% rename from capabilities2_server/launch/capabilities2_server.launch.py rename to capabilities2_server/launch/server.launch.py diff --git a/capabilities2_server/readme.md b/capabilities2_server/readme.md index 62cb0db..4b81b3a 100644 --- a/capabilities2_server/readme.md +++ b/capabilities2_server/readme.md @@ -6,6 +6,19 @@ Capabilities2 server allows interaction with the capabilities2 API. It is a reim 2. Capability registration through service calls. 3. Abstracted providers through a *runners* API see [capabilities2_runner](../capabilities2_runner/readme.md) +## Specialising the capabilities2 server + +The capabilities2 server is implemented as a [ROS2 Component](https://docs.ros.org/en/jazzy/Concepts/Intermediate/About-Composition.html). The server can be specialised by composing it with another Component that adds domain logic. + +## Launch capabilities2 server + +```bash +source install/setup.bash +ros2 launch capabilities2_server server.launch.py +``` + +Refer `capabilities2_server/config/capabilities.yaml` for launch arguments. + ## System dependencies The capabilities2 server depends on the following `bondcpp` ROS2 package. See the package.xml for the full list of dependencies. Notably, the capabilities2 server depends on the following system dependencies: @@ -17,7 +30,7 @@ The capabilities2 server depends on the following `bondcpp` ROS2 package. See th ## API -The capabilities2 server provides the following ROS API: +The capabilities2 server provides the following ROS2 API: ### Services @@ -38,6 +51,8 @@ The capabilities2 server exposes the following Service API (see [capabilities2_m | `~/start_capability` | `StartCapability.srv` | Start a capability (this is a forceful start, and ignores use and free logic) | | `~/stop_capability` | `StopCapability.srv` | Stop a capability (this is a forceful stop, and ignores use and free logic) | | `~/register_capability` | `RegisterCapability.srv` | Register a capability with the capabilities server | +| `~/trigger_capability` | `TriggerCapability.srv` | Trigger a capability | +| `~/configure_capability` | `ConfigureCapability.srv` | Configure a capability with `on_start`, `on_end`, `on_success`, `on_failure` events| ### Topics @@ -49,84 +64,9 @@ The capabilities2 server exposes the following Topics API: | `~/events` | `GetInterfaces.srv` | Publish capability events | | `~/bonds` | `GetSemanticInterfaces.srv` | Maintain bonds with capability users - [Bond API](https://wiki.ros.org/bond) | -## How to use - -### Register a capability - -Capabilities can be registered by exporting them in a package. The capabilities2 server will read the capabilities from the package and make them available to the user. - -```xml - - - - interfaces/cool_cap.yaml - - -``` - -Following is the content of the cool_cap.yaml - -```yaml -# cool_cap.yaml -name: cool_cap -spec_version: 1 -spec_type: interface -description: "This is a cool capability" -interface: - topics: - "/cool_topic": - type: "std_msgs/msg/String" - description: "This is a cool topic" -``` - -Capabilities can also be registered through a service call. This is useful for registering capabilities that are not exported in a package. The service call has been implemented as a node in the capabilities2 package. Use the node to register capabilities during runtime. This method can also be used to update capabilities. - -### Launch capabilities2 server - -```bash -ros2 launch capabilities2_server capabilities2_server.launch.py -``` - -Can add launch args for path to packages for exported capabilties via terminal as well. refer `./config/capabilities.yaml` for example usage. - - -### Terminal based capability usage +
-Using a capability requires the user to establish a bond with the capabilities2 server. The bond is used to maintain a persistent connection with the server. +## Additional Information -first, inspect the available capabilities provided under this server on this robot. - -```bash -ros2 service call /capabilities/get_capability_specs capabilities2_msgs/srv/GetCapabilitySpecs -``` - -then, request a bond id to become a persistent user - -```bash -ros2 service call /capabilities/establish_bond capabilities2_msgs/srv/EstablishBond - -# this bond needs to be updated every second by publishing a heartbeat the bond topic. Replace the ... with value received from above call -ros2 topic pub /capabilities/bonds ... -``` - -then request to use a capability - -```bash -ros2 service call /capabilities/use_capability capabilities2_msgs/srv/UseCapability -``` - -This capability can be freed by calling the `free_capability` service, or just let the bond expire. The capability will be freed automatically. - - -## Developing - -A `devcontainer` is provided for developing the capabilities2 meta-package. Dependencies need to be installed in the container. Install the dependencies with rosdep: - -```bash -# in the devcontainer -rosdep install --from-paths src --ignore-src -r -y -``` - -### Specialising the capabilities2 server - -The capabilities2 server is implemented as a [ROS2 Component](https://docs.ros.org/en/jazzy/Concepts/Intermediate/About-Composition.html). The server can be specialised by composing it with another Component that adds domain logic. +- [Registering a capability](../capabilities2_server/docs/register.md) +- [Terminal based capability usage](../capabilities2_server/docs/terminal_usage.md) diff --git a/docs/design.md b/docs/design.md new file mode 100644 index 0000000..4366e1b --- /dev/null +++ b/docs/design.md @@ -0,0 +1,15 @@ +## Design + +The new capabilities package is designed to be more efficient and extensible. The functions are implemented as plugins, which can be loaded at runtime. The execution of providers is abstracted using an API called runners. The runners can manage more arbitrary provider operation which can include performing actions or services, or even running other capabilities. The capability models are stored in a database, This will allow various feature improvements such as hot reloading, state persistence, and model extension. Another possible feature is to create more complex ontological relationships between capabilities such as prerequisites, conflicts, or RDF triples (for example, `grasp` results in `holding`, or `pick` is a type of `manipulation`, or `grasp` is incompatible with `push`). + +### Bonds + +The bonds feature as implemented in `capabilities` and reimplemented in `capabilities2` allows applications to overlap functions by requesting use of the same resources. When all references to a resource are released, the resource is stopped. This has the added benefit of creating an idle state for the robot in which the robot computing resources are not being used. As a result of this, the robot could be more energy-efficient, and the robot could be more responsive to new tasks. + +### Runners + +The runners feature which is new in `capabilities2` allows capabilities to be executed in a more diverse manner. This could include running actions, services, or even other capabilities. This allows capabilities to be used like actions and not just started and stopped by the capabilities service. The Runner API is designed to be extensible, so that specific runners can be created on a per provider basis. Another feature that can be implemented is cross-runner communication, which could allow capabilities to be combined at runtime to perform more complex tasks. This could overcome limitations in robot programming in which there is a common need for extensive pre-definition of tasks. + +#### launch runner + +To enable the launch functionality from `capabilities` in `capabilities2`, a `launch runner` has been implemented. Due to the design of the launch system in ROS2, it was necessary to create a `launch_proxy` node which uses the `python` launch API to start and stop launch files. The runner allows uses an action to start and stop launch files, and keep track of running launch files. diff --git a/docs/run_test_scripts.md b/docs/run_test_scripts.md index 791200c..77044e3 100644 --- a/docs/run_test_scripts.md +++ b/docs/run_test_scripts.md @@ -1,14 +1,23 @@ # Run test scripts -Capabilities features can be tested using the provided test scripts. The test scripts primarily test the capabilities2 server using service clients. Launch the capabilities2 server before running the test scripts [information here](../capabilities2_server/readme.md). The test scripts are written to test against the `std_capabilities` package. Make sure that the `std_capabilities` package is copied along with the `capabilities2` package to the `capabilities2_ws/src` folder and is built and sourced before running the test scripts. +Capabilities features can be tested using the provided test scripts. The test scripts primarily test the capabilities2 server using service clients. Launch the capabilities2 server before running the test scripts using -> **Note**: The test scripts use the `bondby` package. Make sure to install the `bondpy` package before running the test scripts. -> sudo apt install ros-$ROS_DISTRO-bondy +```bash +source install/setup.bash +ros2 launch capabilities2_server server.launch.py +``` + +The test scripts are written to test against the `std_capabilities` package. Make sure that the `std_capabilities` package is copied along with the `capabilities2` package to the `capabilities2_ws/src` folder and is built and sourced before running the test scripts. + +> **Note**: The test scripts use the `bondby` package. Make sure to install the `bondpy` package before running the test scripts. \ +> `sudo apt install ros-$ROS_DISTRO-bondy` Run the tests with python3. The test scripts are located in the `capabilities2_server/test` directory. Make sure to source the workspace before running the test. ```bash # example +source install/setup.bash +cd src/capabilities2/capabilities2_server/test python3 call_establish_bond.py ``` @@ -23,6 +32,13 @@ python3 call_establish_bond.py There is another test script in the `capabilities2_launch_proxy` package that tests using a capability. +```bash +# example +source install/setup.bash +cd src/capabilities2/capabilities2_launch_proxy/test +python3 call_establish_bond.py +``` + | Test Script | Description | | --- | --- | | [call_use_launch_runner.py](../capabilities2_launch_proxy/test/call_use_launch_runner.py) | Test using a launch runner based capability. This tests the bond, use, and get running features of the capabilities server | diff --git a/docs/setup.md b/docs/setup.md new file mode 100644 index 0000000..b5239fe --- /dev/null +++ b/docs/setup.md @@ -0,0 +1,61 @@ +## Using Capabilities package (Development and Deployment) + +The current package has been tested on Ubuntu and ROS2 Humble, Rolling and Jazzy. Complete following sections in order. + +### Base folder creation + +Create the workspace folder by running following commands in the terminal. + +```bash +mkdir -p /home/$USER/capabilities_ws/src +cd /home/$USER/capabilities_ws/src +``` + +### Cloning the Package + +Clone the package using Git + +```bash +git clone -b develop https://github.com/CollaborativeRoboticsLab/capabilities2.git +``` + +### Basic configuration + +On the terminal run the following command to identify the $USER and note down the value + +```bash +echo $USER +``` + +Then open the `/home/$USER/capabilities_ws/src/capabilities2/capabilities2_server/ config/capabilities.yaml` with an available text editor. Either gedit or nano can be used. + +```sh +nano /home/$USER/capabilities_ws/src/capabilities2/capabilities2_server/config/capabilities.yaml +``` + +In the opened file, replace $USER value in lines 9 & 10 with above identified value for $USER. if the $USER is ubuntu, those lines should be + +```yaml + - /home/ubuntu/capabilities_ws/src + - /home/ubuntu/capabilities_ws/src/capabilities2 +``` + +Save the file and close. + +### Dependency installation + +Move the terminal to workspace root and install dependencies. + +```bash +cd /home/$USER/capabilities_ws +``` +```bash +rosdep install --from-paths src --ignore-src -r -y +``` + +### Compile + +Use colcon to build the packages: +```bash +colcon build +``` \ No newline at end of file diff --git a/docs/setup_with_dev.md b/docs/setup_with_dev.md new file mode 100644 index 0000000..d68c74c --- /dev/null +++ b/docs/setup_with_dev.md @@ -0,0 +1,84 @@ +## Using Capabilities package (Development and Deployment) + +The current package has been tested on Ubuntu and ROS2 Humble, Rolling and Jazzy. Complete following sections in order. + +### Base folder creation + +Create the workspace folder by running following commands in the terminal. + +```bash +mkdir -p /home/$USER/capabilities_ws/src +cd /home/$USER/capabilities_ws/src +``` + +### Cloning the Package + +Clone the package using Git + +```bash +git clone -b develop https://github.com/CollaborativeRoboticsLab/capabilities2.git +``` + +### When using devcontainer + +A `devcontainer` is provided for developing the capabilities2 meta-package. The container can be used with [Microsoft Visual Studio Code](https://code.visualstudio.com/) and [Remote Development Extention](https://marketplace.visualstudio.com/items?itemName=ms-vscode-remote.vscode-remote-extensionpack). Dependencies need to be installed in the container. Install the dependencies with rosdep: + +```bash +# in the devcontainer +rosdep install --from-paths src --ignore-src -r -y +``` + +### When using without the devcontainer + +On the terminal run the following command to identify the $USER and note down the value + +```bash +echo $USER +``` + +Then open the `/home/$USER/ROS/capabilities_ws/src/capabilities2/capabilities2_server/config/capabilities.yaml` with an available text editor. Either gedit or nano can be used. + +```sh +nano /home/$USER/ROS/capabilities_ws/src/capabilities2/capabilities2_server/config/capabilities.yaml +``` + +In the opened file, replace $USER value in lines 9 & 10 with above identified value for $USER. if the $USER is ubuntu, those lines should be + +```yaml + - /home/ubuntu/capabilities_ws/src + - /home/ubuntu/capabilities_ws/src/capabilities2 +``` + +Save the file and close. + +Move the terminal to workspace root and install dependencies. + +```bash +cd /home/$USER/capabilities_ws +``` +```bash +rosdep install --from-paths src --ignore-src -r -y +``` + +### Building + +Use Colcon to build the package: + +```bash +colcon build +``` + +### Starting the Capabilities2 server + +```bash +source install/setup.bash +ros2 launch capabilities2_server capabilities2_server.launch.py +``` + +### Terminal based bond creation + +Read more about this [here](../capabilities2/capabilities2_server/readme.md) + +### Running Test cases + +Read more about this [here](../capabilities2/docs/run_test_scripts.md) \ No newline at end of file diff --git a/docs/testing.md b/docs/testing.md new file mode 100644 index 0000000..5adeead --- /dev/null +++ b/docs/testing.md @@ -0,0 +1,5 @@ + + +### + +Read more about this [here]() \ No newline at end of file diff --git a/readme.md b/readme.md index 91714cd..8af75b1 100644 --- a/readme.md +++ b/readme.md @@ -4,11 +4,13 @@ [![License: MIT](https://img.shields.io/badge/License-MIT-yellow.svg)](https://opensource.org/licenses/MIT) [![Open in Visual Studio Code](https://img.shields.io/badge/vscode-dev-blue)](https://open.vscode.dev/airesearchlab/capabilities2) ![C++](https://img.shields.io/badge/Code-C++-informational?&logo=c%2b%2b) -![ROS](https://img.shields.io/badge/Framework-ROS-informational?&logo=ROS) +![ROS](https://img.shields.io/badge/Framework-ROS2-informational?&logo=ROS) -A reimplementation of the [capabilities](https://github.com/osrf/capabilities) package. This package is implemented using C++17 and extends the capabilities package features. See the [capabilities2_server](./capabilities2_server/readme.md) package for the main system component. +A reimplementation of the [capabilities](https://github.com/osrf/capabilities) package. This package is implemented using C++17 and extends the capabilities package features. +- [capabilities2_server](./capabilities2_server/readme.md) package contains the core of the system. +- [capabilities2_runner](./capabilities2_server/readme.md) package contains base and template classes for capability implementations. +- [capabilities2_fabric](./capabilities2_fabric/readme.md) package implements a control framework that utlizes capabilities2 system. -[More Information about Motivation and Example Use Cases](./docs/motivation_and_examples.md) ## System structure @@ -16,7 +18,7 @@ A reimplementation of the [capabilities](https://github.com/osrf/capabilities) p ## Entities -The main usage of `capabilities2` will typically involve creating capabilities through providers, interfaces and semantic interfaces. Which are represented in specification file stored as YAML, see the definitions and examples for each: +The main usage of `capabilities2` will typically involve creating or customizing capabilities through providers, interfaces and semantic interfaces. These are stored as YAML, and for more information about definitions and examples, click the links: | Entity | Description | | --- | --- | @@ -24,103 +26,37 @@ The main usage of `capabilities2` will typically involve creating capabilities t | [Providers](./docs/providers.md) | The capability provider specification file provides a mechanism to operate the capability | | [Semantic Interfaces](./docs/semantic_interfaces.md) | The semantic interface specification file provides a mechanism to redefine a capability with semantic information | -See [docs](./docs/) for the format of these entities or click the links above. Runners can be created using the runner API parent classes [here](./capabilities2_runner/readme.md). The capabilities service can be started using the [capabilities2_server](./capabilities2_server/readme.md) package. +Runners can be created using the runner API parent classes [here](./capabilities2_runner/readme.md). The capabilities service can be started using the [capabilities2_server](./capabilities2_server/readme.md) package. -## Design -The new capabilities package is designed to be more efficient and extensible. The functions are implemented as plugins, which can be loaded at runtime. The execution of providers is abstracted using an API called runners. The runners can manage more arbitrary provider operation which can include performing actions or services, or even running other capabilities. The capability models are stored in a database, This will allow various feature improvements such as hot reloading, state persistence, and model extension. Another possible feature is to create more complex ontological relationships between capabilities such as prerequisites, conflicts, or RDF triples (for example, `grasp` results in `holding`, or `pick` is a type of `manipulation`, or `grasp` is incompatible with `push`). +## Setup Information +- [Setup Instructions with devcontainer](./docs/setup_with_dev.md) +- [Setup Instructions without devcontainer](./docs/setup.md) -### Bonds +## Startup information -The bonds feature as implemented in `capabilities` and reimplemented in `capabilities2` allows applications to overlap functions by requesting use of the same resources. When all references to a resource are released, the resource is stopped. This has the added benefit of creating an idle state for the robot in which the robot computing resources are not being used. As a result of this, the robot could be more energy-efficient, and the robot could be more responsive to new tasks. - -### Runners - -The runners feature which is new in `capabilities2` allows capabilities to be executed in a more diverse manner. This could include running actions, services, or even other capabilities. This allows capabilities to be used like actions and not just started and stopped by the capabilities service. The Runner API is designed to be extensible, so that specific runners can be created on a per provider basis. Another feature that can be implemented is cross-runner communication, which could allow capabilities to be combined at runtime to perform more complex tasks. This could overcome limitations in robot programming in which there is a common need for extensive pre-definition of tasks. - -#### launch runner - -To enable the launch functionality from `capabilities` in `capabilities2`, a `launch runner` has been implemented. Due to the design of the launch system in ROS2, it was necessary to create a `launch_proxy` node which uses the `python` launch API to start and stop launch files. The runner allows uses an action to start and stop launch files, and keep track of running launch files. - - -## Using Capabilities package (Development and Deployment) - -The current package has been tested on Ubuntu and ROS2 Rolling and Jazzy. - -### Basic setup - -Create the workspace folder by running following commands in the terminal. - -```bash -mkdir -p /home/$USER/capabilities_ws/src -cd /home/$USER/capabilities_ws/src -``` - -Unzip and copy the `capabilities2` folder into the `/home/$USER/capabilities_ws/src` folder. (Final path should be like `/home/$USER/capabilities_ws/src/capabilities2`) - -### When using devcontainer - -A `devcontainer` is provided for developing the capabilities2 meta-package. The container can be used with [Microsoft Visual Studio Code](https://code.visualstudio.com/) and [Remote Development Extention](https://marketplace.visualstudio.com/items?itemName=ms-vscode-remote.vscode-remote-extensionpack). Dependencies need to be installed in the container. Install the dependencies with rosdep: - -```bash -# in the devcontainer -rosdep install --from-paths src --ignore-src -r -y -``` - -### When using without the devcontainer - -On the terminal run the following command to identify the $USER and note down the value - -```bash -echo $USER -``` - -Then open the `/home/$USER/ROS/capabilities_ws/src/capabilities2/capabilities2_server/config/capabilities.yaml` with an available text editor. Either gedit or nano can be used. - -```sh -nano /home/$USER/ROS/capabilities_ws/src/capabilities2/capabilities2_server/config/capabilities.yaml -``` - -In the opened file, replace $USER value in lines 9 & 10 with above identified value for $USER. if the $USER is ubuntu, those lines should be - -```yaml - - /home/ubuntu/capabilities_ws/src - - /home/ubuntu/capabilities_ws/src/capabilities2 -``` - -Save the file and close. - -Move the terminal to workspace root and install dependencies. +### Starting the Capabilities2 server ```bash -cd /home/$USER/capabilities_ws -``` -```bash -rosdep install --from-paths src --ignore-src -r -y +source install/setup.bash +ros2 launch capabilities2_server server.launch.py ``` -### Building - -Use Colcon to build the package: - -```bash -colcon build -``` +### Starting the Capabilities2 Fabric -### Starting the Capabilities2 server +Start the capabilities2 server first. Then run the following on a new terminal ```bash source install/setup.bash -ros2 launch capabilities2_server capabilities2_server.launch.py +ros2 launch capabilities2_fabric fabric.launch.py ``` -### Terminal based bond creation - -Read more about this [here](../capabilities2/capabilities2_server/readme.md) - -### Running Test cases - -Read more about this [here](../capabilities2/docs/run_test_scripts.md) +## Additional Information +- [Motivation and Example Use Cases](./docs/motivation_and_examples.md) +- [Design Information](./docs/design.md) +- [Registering a capability](./capabilities2_server/docs/register.md) +- [Terminal based capability usage](./capabilities2_server/docs/terminal_usage.md) +- [Running test scripts](./docs/run_test_scripts.md) ## Acknowledgements From e6a3a45ed127eb4fe892a342e4a86ff9cce9789b Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Mon, 11 Nov 2024 22:34:37 +1100 Subject: [PATCH 024/133] improved the capability discovery via package.xml not have specific paths in config yaml --- capabilities2_server/config/capabilities.yaml | 6 +- .../capabilities_server.hpp | 111 +++++++++++++++++- docs/setup.md | 24 +--- docs/setup_with_dev.md | 53 +-------- std_capabilities2/CMakeLists.txt | 3 - std_capabilities2/package.xml | 2 +- 6 files changed, 112 insertions(+), 87 deletions(-) diff --git a/capabilities2_server/config/capabilities.yaml b/capabilities2_server/config/capabilities.yaml index fdb3b61..b0f1a87 100644 --- a/capabilities2_server/config/capabilities.yaml +++ b/capabilities2_server/config/capabilities.yaml @@ -4,10 +4,6 @@ db_file: ~/.ros/capabilities/capabilities.sqlite3 rebuild: true # Set to true to rebuild the database package_paths: # paths to search for capabilities + - install/ - /opt/ros/humble/share - /opt/ros/jazzy/share - - /home/$USER/capabilities_ws/src - - /home/$USER/capabilities_ws/src/capabilities2 - - /home/ubuntu/colcon_ws/src - - /home/ubuntu/colcon_ws/src/capabilities2 - - /home/kalana/ROS/capabilities_ws/src/capabilities2 diff --git a/capabilities2_server/include/capabilities2_server/capabilities_server.hpp b/capabilities2_server/include/capabilities2_server/capabilities_server.hpp index d6a7559..f62b5f1 100644 --- a/capabilities2_server/include/capabilities2_server/capabilities_server.hpp +++ b/capabilities2_server/include/capabilities2_server/capabilities_server.hpp @@ -401,39 +401,55 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI } private: - void load_capabilities(const std::string& package_path) { RCLCPP_DEBUG(get_logger(), "Loading capabilities from package path: %s", package_path.c_str()); + // check if path exists if (!std::filesystem::exists(package_path)) { RCLCPP_ERROR(get_logger(), "package path does not exist: %s", package_path.c_str()); return; } + // find packages in path - std::vector packages; + std::vector packages_root, packages_install; + for (const auto& entry : std::filesystem::directory_iterator(package_path)) { if (entry.is_directory()) { - packages.push_back(entry.path().filename()); + std::string name(entry.path().filename()); + + if (std::filesystem::exists(package_path + "/" + name + "/package.xml")) + { + packages_root.push_back(name); + } + else if (std::filesystem::exists(package_path + "/" + name + "/share/" + name + "/package.xml")) + { + packages_install.push_back(name); + } } } - // load capabilities from packages - for (const auto& package : packages) + + // load capabilities from packages in /opt/ros/*/share + for (const auto& package : packages_root) { RCLCPP_DEBUG(get_logger(), "loading capabilities from package: %s", package.c_str()); + // package.xml exports std::string package_xml = package_path + "/" + package + "/package.xml"; + // check if package.xml exists if (!std::filesystem::exists(package_xml)) { RCLCPP_DEBUG(get_logger(), "package.xml does not exist: %s", package_xml.c_str()); continue; } + // parse package.xml tinyxml2::XMLDocument doc; + try { parse_package_xml(package_xml, doc); @@ -443,13 +459,16 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI RCLCPP_ERROR(get_logger(), "failed to parse package.xml file: %s", e.what()); continue; } + // get exports tinyxml2::XMLElement* exports = doc.FirstChildElement("package")->FirstChildElement("export"); + if (exports == nullptr) { RCLCPP_DEBUG(get_logger(), "No exports found in package.xml file: %s", package_xml.c_str()); continue; } + // get capability specs of each type using a lambda auto get_capability_specs = [&](const std::string& spec_type) { // get capability spec @@ -485,6 +504,88 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI } } }; + + // get interface specs + get_capability_specs(capabilities2_msgs::msg::CapabilitySpec::CAPABILITY_INTERFACE); + // get semantic interface specs + get_capability_specs(capabilities2_msgs::msg::CapabilitySpec::SEMANTIC_CAPABILITY_INTERFACE); + // get provider specs + get_capability_specs(capabilities2_msgs::msg::CapabilitySpec::CAPABILITY_PROVIDER); + } + + // load capabilities from packages in workspace install folder + for (const auto& package : packages_install) + { + RCLCPP_DEBUG(get_logger(), "loading capabilities from package: %s", package.c_str()); + + // package.xml exports + std::string package_xml = package_path + "/" + package + "/share/" + package + "/package.xml"; + + // check if package.xml exists + if (!std::filesystem::exists(package_xml)) + { + RCLCPP_DEBUG(get_logger(), "package.xml does not exist: %s", package_xml.c_str()); + continue; + } + + // parse package.xml + tinyxml2::XMLDocument doc; + + try + { + parse_package_xml(package_xml, doc); + } + catch (const std::runtime_error& e) + { + RCLCPP_ERROR(get_logger(), "failed to parse package.xml file: %s", e.what()); + continue; + } + + // get exports + tinyxml2::XMLElement* exports = doc.FirstChildElement("package")->FirstChildElement("export"); + + if (exports == nullptr) + { + RCLCPP_DEBUG(get_logger(), "No exports found in package.xml file: %s", package_xml.c_str()); + continue; + } + + // get capability specs of each type using a lambda + auto get_capability_specs = [&](const std::string& spec_type) { + // get capability spec + for (tinyxml2::XMLElement* spec = exports->FirstChildElement(spec_type.c_str()); spec != nullptr; + spec = spec->NextSiblingElement(spec_type.c_str())) + { + // read spec relative path for spec file from element contents + std::string spec_path = spec->GetText(); + // clear white spaces + spec_path.erase(std::remove_if(spec_path.begin(), spec_path.end(), isspace), spec_path.end()); + // remove leading slash + if (spec_path[0] == '/') + { + spec_path = spec_path.substr(1); + } + // create a spec message + capabilities2_msgs::msg::CapabilitySpec capability_spec; + // add package details + capability_spec.package = package; + capability_spec.type = spec_type; + // try load spec file + try + { + // read spec file + load_spec_content(package_path + "/" + package + "/share/" + package + "/" + spec_path, capability_spec); + // add capability to db + RCLCPP_INFO(get_logger(), "adding capability: %s", (package + "/" + spec_path).c_str()); + add_capability(capability_spec); + } + catch (const std::runtime_error& e) + { + RCLCPP_ERROR(get_logger(), "failed to load spec file: %s", e.what()); + } + } + }; + // get interface specs get_capability_specs(capabilities2_msgs::msg::CapabilitySpec::CAPABILITY_INTERFACE); // get semantic interface specs diff --git a/docs/setup.md b/docs/setup.md index b5239fe..4378c37 100644 --- a/docs/setup.md +++ b/docs/setup.md @@ -19,29 +19,6 @@ Clone the package using Git git clone -b develop https://github.com/CollaborativeRoboticsLab/capabilities2.git ``` -### Basic configuration - -On the terminal run the following command to identify the $USER and note down the value - -```bash -echo $USER -``` - -Then open the `/home/$USER/capabilities_ws/src/capabilities2/capabilities2_server/ config/capabilities.yaml` with an available text editor. Either gedit or nano can be used. - -```sh -nano /home/$USER/capabilities_ws/src/capabilities2/capabilities2_server/config/capabilities.yaml -``` - -In the opened file, replace $USER value in lines 9 & 10 with above identified value for $USER. if the $USER is ubuntu, those lines should be - -```yaml - - /home/ubuntu/capabilities_ws/src - - /home/ubuntu/capabilities_ws/src/capabilities2 -``` - -Save the file and close. - ### Dependency installation Move the terminal to workspace root and install dependencies. @@ -56,6 +33,7 @@ rosdep install --from-paths src --ignore-src -r -y ### Compile Use colcon to build the packages: + ```bash colcon build ``` \ No newline at end of file diff --git a/docs/setup_with_dev.md b/docs/setup_with_dev.md index d68c74c..2d4be30 100644 --- a/docs/setup_with_dev.md +++ b/docs/setup_with_dev.md @@ -19,7 +19,7 @@ Clone the package using Git git clone -b develop https://github.com/CollaborativeRoboticsLab/capabilities2.git ``` -### When using devcontainer +### Devcontainer A `devcontainer` is provided for developing the capabilities2 meta-package. The container can be used with [Microsoft Visual Studio Code](https://code.visualstudio.com/) and [Remote Development Extention](https://marketplace.visualstudio.com/items?itemName=ms-vscode-remote.vscode-remote-extensionpack). Dependencies need to be installed in the container. Install the dependencies with rosdep: @@ -28,57 +28,10 @@ A `devcontainer` is provided for developing the capabilities2 meta-package. The rosdep install --from-paths src --ignore-src -r -y ``` -### When using without the devcontainer +### Compile -On the terminal run the following command to identify the $USER and note down the value - -```bash -echo $USER -``` - -Then open the `/home/$USER/ROS/capabilities_ws/src/capabilities2/capabilities2_server/config/capabilities.yaml` with an available text editor. Either gedit or nano can be used. - -```sh -nano /home/$USER/ROS/capabilities_ws/src/capabilities2/capabilities2_server/config/capabilities.yaml -``` - -In the opened file, replace $USER value in lines 9 & 10 with above identified value for $USER. if the $USER is ubuntu, those lines should be - -```yaml - - /home/ubuntu/capabilities_ws/src - - /home/ubuntu/capabilities_ws/src/capabilities2 -``` - -Save the file and close. - -Move the terminal to workspace root and install dependencies. - -```bash -cd /home/$USER/capabilities_ws -``` -```bash -rosdep install --from-paths src --ignore-src -r -y -``` - -### Building - -Use Colcon to build the package: +Use colcon to build the packages: ```bash colcon build ``` - -### Starting the Capabilities2 server - -```bash -source install/setup.bash -ros2 launch capabilities2_server capabilities2_server.launch.py -``` - -### Terminal based bond creation - -Read more about this [here](../capabilities2/capabilities2_server/readme.md) - -### Running Test cases - -Read more about this [here](../capabilities2/docs/run_test_scripts.md) \ No newline at end of file diff --git a/std_capabilities2/CMakeLists.txt b/std_capabilities2/CMakeLists.txt index bc6e513..5287c56 100644 --- a/std_capabilities2/CMakeLists.txt +++ b/std_capabilities2/CMakeLists.txt @@ -18,7 +18,4 @@ install(DIRECTORY providers DESTINATION share/${PROJECT_NAME} ) -ament_export_include_directories(include) -ament_export_libraries(${PROJECT_NAME}) - ament_package() diff --git a/std_capabilities2/package.xml b/std_capabilities2/package.xml index 7ee6b7d..8bfe758 100644 --- a/std_capabilities2/package.xml +++ b/std_capabilities2/package.xml @@ -5,7 +5,7 @@ 0.0.0 TODO: Package description kalana - TODO: License declaration + BSD ament_cmake From a6bd1b3f1e59cbdd73051ecbc24b25087568dc62 Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Mon, 11 Nov 2024 22:38:42 +1100 Subject: [PATCH 025/133] minor fix --- capabilities2_fabric/CMakeLists.txt | 8 ++++---- capabilities2_fabric/config/executor.yaml | 4 ++-- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/capabilities2_fabric/CMakeLists.txt b/capabilities2_fabric/CMakeLists.txt index 5213f8a..7b5cb25 100644 --- a/capabilities2_fabric/CMakeLists.txt +++ b/capabilities2_fabric/CMakeLists.txt @@ -43,22 +43,22 @@ install(TARGETS ${PROJECT_NAME} DESTINATION lib/${PROJECT_NAME} ) -add_executable(capabilities_file_parser +add_executable(capabilities2_file_parser src/capabilities_file_parser.cpp ) -target_link_libraries(capabilities_file_parser +target_link_libraries(capabilities2_file_parser ${TINYXML2_LIBRARIES} ) -ament_target_dependencies(capabilities_file_parser +ament_target_dependencies(capabilities2_file_parser rclcpp rclcpp_action capabilities2_msgs TINYXML2 ) -install(TARGETS capabilities_file_parser +install(TARGETS capabilities2_file_parser DESTINATION lib/${PROJECT_NAME} ) diff --git a/capabilities2_fabric/config/executor.yaml b/capabilities2_fabric/config/executor.yaml index 07a0f29..8dbc6e8 100644 --- a/capabilities2_fabric/config/executor.yaml +++ b/capabilities2_fabric/config/executor.yaml @@ -1,4 +1,4 @@ /**: ros__parameters: - plan_file_path: "install/capabilities2_executor/share/capabilities2_executor/plans/navigation_1.xml" - # plan_file_path: "install/capabilities2_executor/share/capabilities2_executor/plans/default.xml" + plan_file_path: "install/capabilities2_fabric/share/capabilities2_fabric/plans/navigation_1.xml" + # plan_file_path: "install/capabilities2_fabric/share/capabilities2_fabric/plans/default.xml" From 7ace6bc20711d8e8cd52d2a721972b46d9cb807f Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Wed, 13 Nov 2024 12:49:00 +1100 Subject: [PATCH 026/133] work-in-progress commit. rewrote fabric to work asynchronously but multiple interdependent ros2 service clients.fixed issue with dynamically linking tinyXML2 and solved with statically linking that --- capabilities2_fabric/CMakeLists.txt | 21 +- .../capabilities_fabric.hpp | 822 +++++++++++------- .../capabilities_file_parser.hpp | 29 +- .../capabilities_xml_parser.hpp | 7 +- capabilities2_fabric/launch/fabric.launch.py | 6 +- .../src/capabilities_fabric.cpp | 13 +- .../src/capabilities_file_parser.cpp | 10 +- capabilities2_msgs/action/Plan.action | 2 + capabilities2_runner/CMakeLists.txt | 13 +- capabilities2_runner_audio/CMakeLists.txt | 13 +- capabilities2_runner_bt/CMakeLists.txt | 13 +- capabilities2_runner_nav2/CMakeLists.txt | 13 +- capabilities2_runner_prompt/CMakeLists.txt | 13 +- .../prompt_plan_response_runner.hpp | 2 +- capabilities2_server/CMakeLists.txt | 19 +- 15 files changed, 607 insertions(+), 389 deletions(-) diff --git a/capabilities2_fabric/CMakeLists.txt b/capabilities2_fabric/CMakeLists.txt index 7b5cb25..e7f275c 100644 --- a/capabilities2_fabric/CMakeLists.txt +++ b/capabilities2_fabric/CMakeLists.txt @@ -16,12 +16,15 @@ find_package(rclcpp REQUIRED) find_package(rclcpp_action REQUIRED) find_package(capabilities2_msgs REQUIRED) -find_package(PkgConfig) -pkg_check_modules(TINYXML2 tinyxml2 REQUIRED) +# Locate the static version of tinyxml2 +find_library(TINYXML2_LIB NAMES tinyxml2 PATHS /usr/local/lib NO_DEFAULT_PATH) -include_directories( - include - ${TINYXML2_INCLUDE_DIRS} +if(NOT TINYXML2_LIB) + message(FATAL_ERROR "tinyxml2 library not found. Make sure it's installed as a static library.") +endif() + +include_directories(include + /usr/local/include ) add_executable(${PROJECT_NAME} @@ -29,18 +32,17 @@ add_executable(${PROJECT_NAME} ) target_link_libraries(${PROJECT_NAME} - ${TINYXML2_LIBRARIES} + ${TINYXML2_LIB} ) ament_target_dependencies(${PROJECT_NAME} rclcpp rclcpp_action capabilities2_msgs - TINYXML2 ) install(TARGETS ${PROJECT_NAME} - DESTINATION lib/${PROJECT_NAME} + DESTINATION lib/${PROJECT_NAME} ) add_executable(capabilities2_file_parser @@ -48,14 +50,13 @@ add_executable(capabilities2_file_parser ) target_link_libraries(capabilities2_file_parser - ${TINYXML2_LIBRARIES} + ${TINYXML2_LIB} ) ament_target_dependencies(capabilities2_file_parser rclcpp rclcpp_action capabilities2_msgs - TINYXML2 ) install(TARGETS capabilities2_file_parser diff --git a/capabilities2_fabric/include/capabilities2_executor/capabilities_fabric.hpp b/capabilities2_fabric/include/capabilities2_executor/capabilities_fabric.hpp index bb368a4..25c76ca 100644 --- a/capabilities2_fabric/include/capabilities2_executor/capabilities_fabric.hpp +++ b/capabilities2_fabric/include/capabilities2_executor/capabilities_fabric.hpp @@ -22,34 +22,126 @@ #include /** - * @brief Capabilities Executor + * @brief Capabilities Fabric * - * Capabilities executor node that provides a ROS client for the capabilities server. + * Capabilities fabric node that provides a ROS client for the capabilities server. * Able to receive a XML file that implements a plan via action server that it exposes - * or via a file read. * */ class CapabilitiesFabric : public rclcpp::Node { public: - CapabilitiesFabric() : Node("Capabilities2_Executor") + using Plan = capabilities2_msgs::action::Plan; + using GoalHandlePlan = rclcpp_action::ServerGoalHandle; + + using GetInterfaces = capabilities2_msgs::srv::GetInterfaces; + using GetSemanticInterfaces = capabilities2_msgs::srv::GetSemanticInterfaces; + using GetProviders = capabilities2_msgs::srv::GetProviders; + using EstablishBond = capabilities2_msgs::srv::EstablishBond; + using UseCapability = capabilities2_msgs::srv::UseCapability; + using FreeCapability = capabilities2_msgs::srv::FreeCapability; + using ConfigureCapability = capabilities2_msgs::srv::ConfigureCapability; + using TriggerCapability = capabilities2_msgs::srv::TriggerCapability; + + using GetInterfacesClient = rclcpp::Client; + using GetSemanticInterfacesClient = rclcpp::Client; + using GetProvidersClient = rclcpp::Client; + using EstablishBondClient = rclcpp::Client; + using UseCapabilityClient = rclcpp::Client; + using FreeCapabilityClient = rclcpp::Client; + using ConfigureCapabilityClient = rclcpp::Client; + using TriggerCapabilityClient = rclcpp::Client; + + CapabilitiesFabric() : Node("Capabilities2_Fabric") { control_tag_list = capabilities2_xml_parser::get_control_list(); + } - this->planner_server_ = rclcpp_action::create_server( - this, "~/capabilities_fabric", std::bind(&CapabilitiesFabric::handle_goal, this, std::placeholders::_1, std::placeholders::_2), + /** + * @brief Initializer function + * + */ + void initialize() + { + this->planner_server_ = rclcpp_action::create_server( + this, "/capabilities_fabric", std::bind(&CapabilitiesFabric::handle_goal, this, std::placeholders::_1, std::placeholders::_2), std::bind(&CapabilitiesFabric::handle_cancel, this, std::placeholders::_1), std::bind(&CapabilitiesFabric::handle_accepted, this, std::placeholders::_1)); - get_interfaces_client_ = this->create_client("~/get_interfaces"); - get_sem_interf_client_ = this->create_client("~/get_semantic_interfaces"); - get_providers_client_ = this->create_client("~/get_providers"); - establish_bond_client_ = this->create_client("~/establish_bond"); - use_capability_client_ = this->create_client("~/use_capability"); - free_capability_client_ = this->create_client("~/free_capability"); - trigger_capability_client_ = this->create_client("~/trigger_capability"); - configure_capability_client_ = this->create_client("~/configure_capability"); + get_interfaces_client_ = this->create_client("/capabilities/get_interfaces"); + get_sem_interf_client_ = this->create_client("/capabilities/get_semantic_interfaces"); + get_providers_client_ = this->create_client("/capabilities/get_providers"); + establish_bond_client_ = this->create_client("/capabilities/establish_bond"); + use_capability_client_ = this->create_client("/capabilities/use_capability"); + free_capability_client_ = this->create_client("/capabilities/free_capability"); + trigger_capability_client_ = this->create_client("/capabilities/trigger_capability"); + configure_capability_client_ = this->create_client("/capabilities/configure_capability"); + + // Wait for services to become available + while (!get_interfaces_client_->wait_for_service(std::chrono::seconds(1))) + { + RCLCPP_ERROR(this->get_logger(), "/capabilities/get_interfaces not available"); + rclcpp::shutdown(); + return; + } + RCLCPP_INFO(this->get_logger(), "/capabilities/get_interfaces connected"); + + while (!get_sem_interf_client_->wait_for_service(std::chrono::seconds(1))) + { + RCLCPP_ERROR(this->get_logger(), "/capabilities/get_semantic_interfaces not available"); + rclcpp::shutdown(); + return; + } + RCLCPP_INFO(this->get_logger(), "/capabilities/get_semantic_interfaces connected"); + + while (!get_providers_client_->wait_for_service(std::chrono::seconds(1))) + { + RCLCPP_ERROR(this->get_logger(), "/capabilities/get_providers not available"); + rclcpp::shutdown(); + return; + } + RCLCPP_INFO(this->get_logger(), "/capabilities/get_providers connected"); + + while (!establish_bond_client_->wait_for_service(std::chrono::seconds(1))) + { + RCLCPP_ERROR(this->get_logger(), "/capabilities/establish_bond not available"); + rclcpp::shutdown(); + return; + } + RCLCPP_INFO(this->get_logger(), "/capabilities/establish_bond connected"); + + while (!use_capability_client_->wait_for_service(std::chrono::seconds(1))) + { + RCLCPP_ERROR(this->get_logger(), "/capabilities/use_capability not available"); + rclcpp::shutdown(); + return; + } + RCLCPP_INFO(this->get_logger(), "/capabilities/use_capability connected"); + + while (!free_capability_client_->wait_for_service(std::chrono::seconds(1))) + { + RCLCPP_ERROR(this->get_logger(), "/capabilities/free_capability not available"); + rclcpp::shutdown(); + return; + } + RCLCPP_INFO(this->get_logger(), "/capabilities/free_capability connected"); + + while (!trigger_capability_client_->wait_for_service(std::chrono::seconds(1))) + { + RCLCPP_ERROR(this->get_logger(), "/capabilities/trigger_capability not available"); + rclcpp::shutdown(); + return; + } + RCLCPP_INFO(this->get_logger(), "/capabilities/trigger_capability connected"); + + while (!configure_capability_client_->wait_for_service(std::chrono::seconds(1))) + { + RCLCPP_ERROR(this->get_logger(), "/capabilities/configure_capability not available"); + rclcpp::shutdown(); + return; + } + RCLCPP_INFO(this->get_logger(), "/capabilities/configure_capability connected"); } private: @@ -61,7 +153,7 @@ class CapabilitiesFabric : public rclcpp::Node * @param goal pointer to the action goal message * @return rclcpp_action::GoalResponse */ - rclcpp_action::GoalResponse handle_goal(const rclcpp_action::GoalUUID& uuid, std::shared_ptr goal) + rclcpp_action::GoalResponse handle_goal(const rclcpp_action::GoalUUID& uuid, std::shared_ptr goal) { RCLCPP_INFO(this->get_logger(), "Received the goal request with the plan"); (void)uuid; @@ -76,6 +168,8 @@ class CapabilitiesFabric : public rclcpp::Node return rclcpp_action::GoalResponse::REJECT; } + RCLCPP_INFO(this->get_logger(), "Plan parsed and accepted"); + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; } @@ -85,7 +179,7 @@ class CapabilitiesFabric : public rclcpp::Node * @param goal_handle pointer to the action goal handle * @return rclcpp_action::GoalResponse */ - rclcpp_action::CancelResponse handle_cancel(const std::shared_ptr> goal_handle) + rclcpp_action::CancelResponse handle_cancel(const std::shared_ptr goal_handle) { RCLCPP_INFO(this->get_logger(), "Received the request to cancel the plan"); (void)goal_handle; @@ -98,130 +192,245 @@ class CapabilitiesFabric : public rclcpp::Node * * @param goal_handle pointer to the action goal handle */ - void handle_accepted(const std::shared_ptr> goal_handle) + void handle_accepted(const std::shared_ptr goal_handle) { - fabric_thread = std::make_unique(std::bind(&CapabilitiesFabric::execute_plan, this, std::placeholders::_1), goal_handle); + RCLCPP_INFO(this->get_logger(), "Execution started"); + execution(goal_handle); } /** - * @brief request the interfaces, semantic_interfaces and providers from the capabilities2 server - * - * @return `true` if interface retreival is successful,`false` otherwise + * @brief Trigger execution + */ + void execution(const std::shared_ptr goal_handle) + { + expected_providers_ = 0; + completed_providers_ = 0; + + getInterfaces(goal_handle); + } + + /** + * @brief Get Interfaces */ - bool request_information() + void getInterfaces(const std::shared_ptr goal_handle) { - // create request messages - auto request_interface = std::make_shared(); - auto request_sematic = std::make_shared(); - auto request_providers = std::make_shared(); + auto feedback = std::make_shared(); + feedback->progress = "Requesting Interface information..."; + goal_handle->publish_feedback(feedback); + + RCLCPP_INFO(this->get_logger(), "Requesting Interface information..."); + + auto request_interface = std::make_shared(); // request data from the server - auto result_future = get_interfaces_client_->async_send_request(request_interface); + auto result_future = + get_interfaces_client_->async_send_request(request_interface, [this, goal_handle, feedback](GetInterfacesClient::SharedFuture future) { + auto result = std::make_shared(); - RCLCPP_INFO(this->get_logger(), "Requesting Interface information from Capabilities2 Server"); + if (!future.valid()) + { + RCLCPP_ERROR(this->get_logger(), "Failed to get Interface information"); - // wait until data is received - if (rclcpp::spin_until_future_complete(shared_from_this(), result_future) != rclcpp::FutureReturnCode::SUCCESS) - { - RCLCPP_INFO(this->get_logger(), "Failed to receive Interface information from Capabilities2 Server"); - return false; - } + result->success = false; + result->message = "Failed to get Interface information"; + goal_handle->abort(result); - RCLCPP_INFO(this->get_logger(), "Received Interface information from Capabilities2 Server"); + RCLCPP_INFO(this->get_logger(), "Server Execution Cancelled"); + return; + } - // request semantic interfaces available for each and every interface got from the server - for (const auto& interface : result_future.get()->interfaces) - { - request_sematic->interface = interface; + auto response = future.get(); + RCLCPP_INFO(this->get_logger(), "Received Interface information"); - // request semantic interface from the server - auto result_semantic_future = get_sem_interf_client_->async_send_request(request_sematic); + // Process each interface and get Semantic interfaces + for (const auto& interface : response->interfaces) + { + getSemanticInterfaces(interface, goal_handle); + } - RCLCPP_INFO(this->get_logger(), "Requesting Semantic Interface information from Capabilities2 Server for %s", interface.c_str()); + RCLCPP_INFO(this->get_logger(), "Interface retreival successful"); + }); + } - // wait until data is received - if (rclcpp::spin_until_future_complete(shared_from_this(), result_semantic_future) != rclcpp::FutureReturnCode::SUCCESS) - { - RCLCPP_INFO(this->get_logger(), "Failed to receive Semantic Interface information from Capabilities2 Server for %s", interface.c_str()); - return false; - } + /** + * @brief Get Semantic Interfaces + */ + void getSemanticInterfaces(const std::string& interface, const std::shared_ptr goal_handle) + { + auto feedback = std::make_shared(); + feedback->progress = "Requesting semantic interfaces for " + interface; + goal_handle->publish_feedback(feedback); - RCLCPP_INFO(this->get_logger(), "Received Semantic Interface information from Capabilities2 Server for %s", interface.c_str()); + RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); - // add sematic interfaces to the list if available - if (result_semantic_future.get()->semantic_interfaces.size() > 0) - { - for (const auto& semantic_interface : result_semantic_future.get()->semantic_interfaces) - { - interface_list.push_back(semantic_interface); + auto request_sematic = std::make_shared(); + request_sematic->interface = interface; - // request providers of the semantic interface - request_providers->interface = semantic_interface; - request_providers->include_semantic = true; + // request semantic interface from the server + auto result_semantic_future = get_sem_interf_client_->async_send_request( + request_sematic, [this, goal_handle, feedback, interface](GetSemanticInterfacesClient::SharedFuture future) { + if (!future.valid()) + { + RCLCPP_ERROR(this->get_logger(), "Failed to get Semantic Interface information"); + + auto result = std::make_shared(); + result->success = false; + result->message = "Failed to get Semantic Interface information"; + goal_handle->abort(result); - auto result_providers_future = get_providers_client_->async_send_request(request_providers); + RCLCPP_INFO(this->get_logger(), "Server Execution Cancelled"); + return; + } - RCLCPP_INFO(this->get_logger(), "Requesting Providers information from Capabilities2 Server for semantic interface %s", - semantic_interface.c_str()); + auto response = future.get(); + RCLCPP_INFO(this->get_logger(), "Received Semantic Interface information"); - // wait until data is received - if (rclcpp::spin_until_future_complete(shared_from_this(), result_providers_future) != rclcpp::FutureReturnCode::SUCCESS) + if (response->semantic_interfaces.size() > 0) { - RCLCPP_INFO(this->get_logger(), "Failed to receive Providers information from Capabilities2 Server for %s", semantic_interface.c_str()); - return false; + for (const auto& semantic_interface : response->semantic_interfaces) + { + expected_providers_++; + interface_list.push_back(semantic_interface); + getProvider(semantic_interface, goal_handle, true); + } + } + // if no semantic interfaces are availble for a given interface, add the interface instead + else + { + expected_providers_++; + interface_list.push_back(interface); + getProvider(interface, goal_handle, false); } + }); + } - RCLCPP_INFO(this->get_logger(), "Received Providers information from Capabilities2 Server for %s", semantic_interface.c_str()); + /** + * @brief Get Providers + * + */ + void getProvider(const std::string& interface, const std::shared_ptr goal_handle, bool include_semantic) + { + auto feedback = std::make_shared(); + feedback->progress = "Requesting provider for " + interface; + goal_handle->publish_feedback(feedback); + RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); + + auto request_providers = std::make_shared(); + + // request providers of the semantic interface + request_providers->interface = interface; + request_providers->include_semantic = include_semantic; + + auto result_providers_future = get_providers_client_->async_send_request( + request_providers, [this, interface, goal_handle, feedback](GetProvidersClient::SharedFuture future) { + if (!future.valid()) + { + RCLCPP_ERROR(this->get_logger(), "Failed to receive provider for interface: %s", interface.c_str()); + + auto result = std::make_shared(); + result->success = false; + result->message = "Failed to retrieve providers for interface: " + interface; + + goal_handle->abort(result); + return; + } + + auto response = future.get(); + RCLCPP_INFO(this->get_logger(), "Received Providers for Interface: %s", interface.c_str()); + + completed_providers_++; // add defualt provider to the list - providers_list.push_back(result_providers_future.get()->default_provider); + providers_list.push_back(response->default_provider); // add additional providers to the list if available - if (result_providers_future.get()->providers.size() > 0) + if (response->providers.size() > 0) { - for (const auto& provider : result_providers_future.get()->providers) + for (const auto& provider : response->providers) { providers_list.push_back(provider); } } - } - } - // if no semantic interfaces are availble for a given interface, add the interface instead - else - { - interface_list.push_back(interface); - // request providers of the semantic interface - request_providers->interface = interface; - request_providers->include_semantic = false; + // Check if all expected calls are completed before calling verify_plan + if (completed_providers_ == expected_providers_) + { + auto feedback = std::make_shared(); + feedback->progress = "All requested provider data recieved"; + goal_handle->publish_feedback(feedback); + RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); - auto result_providers_future = get_providers_client_->async_send_request(request_providers); + verify_and_continue(goal_handle); + } + }); + } - RCLCPP_INFO(this->get_logger(), "Requesting Providers information from Capabilities2 Server for semantic interface %s", interface.c_str()); + /** + * @brief Verify the plan and continue the execution + * + */ + void verify_and_continue(const std::shared_ptr goal_handle) + { + auto feedback = std::make_shared(); + feedback->progress = "Verifying the plan"; + goal_handle->publish_feedback(feedback); + RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); - // wait until data is received - if (rclcpp::spin_until_future_complete(shared_from_this(), result_providers_future) != rclcpp::FutureReturnCode::SUCCESS) - { - RCLCPP_INFO(this->get_logger(), "Failed to receive Providers information from Capabilities2 Server for %s", interface.c_str()); - return false; - } + // verify the plan + if (!verify_plan()) + { + feedback->progress = "Plan verification failed"; + goal_handle->publish_feedback(feedback); + RCLCPP_ERROR(this->get_logger(), feedback->progress.c_str()); - RCLCPP_INFO(this->get_logger(), "Received Providers information from Capabilities2 Server for %s", interface.c_str()); + auto result = std::make_shared(); - providers_list.push_back(result_providers_future.get()->default_provider); + if (rejected_list.size() > 0) + { + // TODO: improve with error codes + result->success = false; + result->message = "Plan verification failed. There are mismatched events"; - // add sematic interfaces to the list if available - if (result_providers_future.get()->providers.size() > 0) + for (const auto& rejected_element : rejected_list) { - for (const auto& provider : result_providers_future.get()->providers) - { - providers_list.push_back(provider); - } + RCLCPP_ERROR(this->get_logger(), "Failed Events : %s", rejected_element.c_str()); + result->failed_elements.push_back(rejected_element); } + + goal_handle->abort(result); + RCLCPP_ERROR(this->get_logger(), "Server Execution Cancelled"); } + else + { + // TODO: improve with error codes + result->success = false; + result->message = "Plan verification failed."; + goal_handle->abort(result); + } + + RCLCPP_ERROR(this->get_logger(), "Server Execution Cancelled"); } - return true; + feedback->progress = "Plan verification successful"; + goal_handle->publish_feedback(feedback); + RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); + + // extract the plan from the XMLDocument + tinyxml2::XMLElement* plan = capabilities2_xml_parser::get_plan(document); + + feedback->progress = "Plan conversion successful"; + goal_handle->publish_feedback(feedback); + RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); + + // Extract the connections from the plan + capabilities2_xml_parser::extract_connections(plan, connection_map); + + feedback->progress = "Connection extraction successful"; + goal_handle->publish_feedback(feedback); + RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); + + // estasblish the bond with the server + establish_bond(goal_handle); } /** @@ -258,30 +467,53 @@ class CapabilitiesFabric : public rclcpp::Node /** * @brief establish the bond with capabilities2 server * - * @return `true` if bond establishing is successful,`false` otherwise */ - bool establish_bond() + void establish_bond(const std::shared_ptr goal_handle) { + auto feedback = std::make_shared(); + feedback->progress = "Requesting bond id"; + goal_handle->publish_feedback(feedback); + + RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); + // create bond establishing server request - auto request_bond = std::make_shared(); + auto request_bond = std::make_shared(); // send the request - auto result_future = establish_bond_client_->async_send_request(request_bond); + auto result_future = establish_bond_client_->async_send_request( + request_bond, [this, goal_handle, feedback](EstablishBondClient::SharedFuture future) { + if (!future.valid()) + { + RCLCPP_ERROR(this->get_logger(), "Failed to receive the bond id"); - RCLCPP_INFO(this->get_logger(), "Requesting to establish bond with Capabilities2 Server"); + auto result = std::make_shared(); + result->success = false; + result->message = "Failed to retrieve the bond id"; + RCLCPP_ERROR(this->get_logger(), "Server Execution Cancelled"); - // wait for the result - if (rclcpp::spin_until_future_complete(shared_from_this(), result_future) != rclcpp::FutureReturnCode::SUCCESS) - { - RCLCPP_INFO(this->get_logger(), "Failed to establish bond with Capabilities2 Server"); - return false; - } + goal_handle->abort(result); + return; + } - RCLCPP_INFO(this->get_logger(), "Established bond with Capabilities2 Server"); + auto response = future.get(); + bond_id = response->bond_id; - bond_id = result_future.get()->bond_id; + feedback->progress = "Received the bond id"; + goal_handle->publish_feedback(feedback); + RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); - return true; + // start all runners and interfaces that the connections depend on + for (const auto& [key, value] : connection_map) + { + feedback->progress = "Starting capability of Node " + std::to_string(key) + " named " + value.source.runner; + goal_handle->publish_feedback(feedback); + RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); + + expected_capabilities_++; + + use_capability(value.source.runner, value.source.provider, goal_handle); + } + }); } /** @@ -290,78 +522,117 @@ class CapabilitiesFabric : public rclcpp::Node * @param capability capability name to be started * @param provider provider of the capability * @param bond_id bond_id for the capability - * - * @return `true` if use of capability is successful,`false` otherwise */ - bool use_capability(const std::string& capability, const std::string& provider, const std::string& bond_id) + void use_capability(const std::string& capability, const std::string& provider, const std::shared_ptr goal_handle) { - auto request_use = std::make_shared(); + auto feedback = std::make_shared(); + feedback->progress = "Using capability " + capability + " from " + provider; + goal_handle->publish_feedback(feedback); + RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); + auto request_use = std::make_shared(); request_use->capability = capability; request_use->preferred_provider = provider; request_use->bond_id = bond_id; // send the request - auto result_future = use_capability_client_->async_send_request(request_use); + auto result_future = use_capability_client_->async_send_request( + request_use, [this, goal_handle, feedback, capability, provider](UseCapabilityClient::SharedFuture future) { + if (!future.valid()) + { + auto result = std::make_shared(); + result->success = false; + result->message = "Failed to Use capability " + capability + " from " + provider; + goal_handle->abort(result); + + RCLCPP_ERROR(this->get_logger(), result->message.c_str()); + RCLCPP_ERROR(this->get_logger(), "Server Execution Cancelled"); + return; + } - RCLCPP_INFO(this->get_logger(), "Requesting to use %s capability from Capabilities2 Server", capability.c_str()); + completed_capabilities_++; - // wait for the result - if (rclcpp::spin_until_future_complete(shared_from_this(), result_future) != rclcpp::FutureReturnCode::SUCCESS) - { - RCLCPP_INFO(this->get_logger(), "Failed to use requested capability from Capabilities2 Server"); - return false; - } + auto response = future.get(); + feedback->progress = "Successfully used capability " + capability + " from " + provider; + goal_handle->publish_feedback(feedback); + RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); - RCLCPP_INFO(this->get_logger(), "Successfully used requested capability from Capabilities2 Server"); + // Check if all expected calls are completed before calling verify_plan + if (completed_capabilities_ == expected_capabilities_) + { + feedback->progress = "All requested capabilities have started"; + goal_handle->publish_feedback(feedback); + RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); - return true; + feedback->progress = "Configuring the capabilities with events"; + goal_handle->publish_feedback(feedback); + RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); + + configure_capabilities(goal_handle); + } + else + { + // release all capabilities that were used since not all started successfully + for (const auto& [key, value] : connection_map) + { + feedback->progress = "Freeing capability of Node " + std::to_string(key) + " named " + value.source.runner; + goal_handle->publish_feedback(feedback); + RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); + + free_capability(value.source.runner, goal_handle); + } + } + }); } /** * @brief Request use of capability from capabilities2 server * * @param capability capability name to be started - * @param bond_id bond_id for the capability - * - * @return `true` if use of capability is successful,`false` otherwise */ - bool free_capability(const std::string& capability, const std::string& bond_id) + void free_capability(const std::string& capability, const std::shared_ptr goal_handle) { - auto request_free = std::make_shared(); + auto feedback = std::make_shared(); + auto request_free = std::make_shared(); request_free->capability = capability; request_free->bond_id = bond_id; // send the request - auto result_future = free_capability_client_->async_send_request(request_free); - - RCLCPP_INFO(this->get_logger(), "Requesting to free %s capability from Capabilities2 Server", capability.c_str()); - - // wait for the result - if (rclcpp::spin_until_future_complete(shared_from_this(), result_future) != rclcpp::FutureReturnCode::SUCCESS) - { - RCLCPP_INFO(this->get_logger(), "Failed to free requested capability from Capabilities2 Server"); - return false; - } + auto result_future = free_capability_client_->async_send_request( + request_free, [this, goal_handle, capability, feedback](FreeCapabilityClient::SharedFuture future) { + if (!future.valid()) + { + auto result = std::make_shared(); + result->success = false; + result->message = "Failed to free capability " + capability; + goal_handle->abort(result); - RCLCPP_INFO(this->get_logger(), "Successfully freed requested capability from Capabilities2 Server"); + RCLCPP_ERROR(this->get_logger(), result->message.c_str()); + return; + } - return true; + auto response = future.get(); + feedback->progress = "Successfully freed capability " + capability; + goal_handle->publish_feedback(feedback); + RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); + }); } /** * @brief Request use of capability from capabilities2 server - * - * @return `true` if configuration of capability is successful,`false` otherwise */ - bool configure_capabilities() + void configure_capabilities(const std::shared_ptr goal_handle) { + auto feedback = std::make_shared(); + for (const auto& [key, value] : connection_map) { - auto request_configure = std::make_shared(); + auto request_configure = std::make_shared(); - RCLCPP_INFO(this->get_logger(), "Configuring Node : %i", key); + feedback->progress = "Configuring capability of Node " + std::to_string(key) + " named " + value.source.runner; + goal_handle->publish_feedback(feedback); + RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); if (capabilities2_xml_parser::convert_to_string(value.source.parameters, request_configure->source.parameters)) { @@ -438,200 +709,81 @@ class CapabilitiesFabric : public rclcpp::Node request_configure->target_on_failure.provider = ""; } + expected_configurations_++; + // send the request - auto result_future = configure_capability_client_->async_send_request(request_configure); + auto result_future = configure_capability_client_->async_send_request( + request_configure, + [this, goal_handle, value, feedback](ConfigureCapabilityClient::SharedFuture future) { + if (!future.valid()) + { + auto result = std::make_shared(); + result->success = false; + result->message = "Failed to configure capability :" + value.source.runner; + goal_handle->abort(result); + + RCLCPP_ERROR(this->get_logger(), result->message.c_str()); + RCLCPP_ERROR(this->get_logger(), "Server Execution Cancelled"); + return; + } - RCLCPP_INFO(this->get_logger(), "Configuring %s capability from Capabilities2 Server", request_configure->source.capability.c_str()); + completed_configurations_++; - // wait for the result - if (rclcpp::spin_until_future_complete(shared_from_this(), result_future) != rclcpp::FutureReturnCode::SUCCESS) - { - RCLCPP_INFO(this->get_logger(), "Failed to configure requested capability from Capabilities2 Server"); - return false; - } + auto response = future.get(); + feedback->progress = "Successfully configure capability :" + value.source.runner; + goal_handle->publish_feedback(feedback); + RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); - RCLCPP_INFO(this->get_logger(), "Successfully configured requested capability from Capabilities2 Server"); - } + // Check if all expected calls are completed before calling verify_plan + if (completed_configurations_ == expected_configurations_) + { + feedback->progress = "All requested capabilities have been configured"; + goal_handle->publish_feedback(feedback); + RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); - return true; + feedback->progress = "Triggering the first capability"; + goal_handle->publish_feedback(feedback); + RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); + + trigger_first_node(goal_handle); + } + }); + } } /** * @brief Trigger the first node - * - * @return `true` if triggering is successful,`false` otherwise */ - bool trigger_first_node() + void trigger_first_node(const std::shared_ptr goal_handle) { - auto request_trigger = std::make_shared(); + auto feedback = std::make_shared(); + + auto request_trigger = std::make_shared(); std::string parameter_string; capabilities2_xml_parser::convert_to_string(connection_map[0].source.parameters, parameter_string); - request_trigger->capability = connection_map[0].source.runner; request_trigger->parameters = parameter_string; // send the request - auto result_future = trigger_capability_client_->async_send_request(request_trigger); - - RCLCPP_INFO(this->get_logger(), "Requesting to trigger %s capability from Capabilities2 Server", connection_map[0].source.runner.c_str()); - - // wait for the result - if (rclcpp::spin_until_future_complete(shared_from_this(), result_future) != rclcpp::FutureReturnCode::SUCCESS) - { - RCLCPP_INFO(this->get_logger(), "Failed to trigger requested capability from Capabilities2 Server"); - return false; - } - - RCLCPP_INFO(this->get_logger(), "Successfully triggered requested capability from Capabilities2 Server"); - - return true; - } - - /** - * @brief execute the plan - * - * @param server_goal_handle goal handle of the server - */ - void execute_plan(const std::shared_ptr> server_goal_handle) - { - auto result = std::make_shared(); - - RCLCPP_INFO(this->get_logger(), "Execution started"); - - // verify the plan - if (!request_information()) - { - RCLCPP_INFO(this->get_logger(), "Interface retreival failed"); - - // TODO: improve with error codes - result->success = false; - server_goal_handle->canceled(result); - - RCLCPP_INFO(this->get_logger(), "Server Execution Cancelled"); - } - - RCLCPP_INFO(this->get_logger(), "Interface retreival successful"); - - // verify the plan - if (!verify_plan()) - { - RCLCPP_INFO(this->get_logger(), "Plan verification failed"); - - if (rejected_list.size() > 0) - { - // TODO: improve with error codes - result->success = false; - - for (const auto& rejected_element : rejected_list) - { - RCLCPP_ERROR(this->get_logger(), "Failed Events : %s", rejected_element.c_str()); - result->failed_elements.push_back(rejected_element); - } - - server_goal_handle->canceled(result); - RCLCPP_ERROR(this->get_logger(), "Server Execution Cancelled"); - } - else - { - // TODO: improve with error codes - result->success = false; - server_goal_handle->canceled(result); - } - - RCLCPP_INFO(this->get_logger(), "Server Execution Cancelled"); - } - - RCLCPP_INFO(this->get_logger(), "Plan verification successful"); - - // extract the plan from the XMLDocument - tinyxml2::XMLElement* plan = capabilities2_xml_parser::get_plan(document); - - RCLCPP_INFO(this->get_logger(), "Plan conversion successful"); - - // Extract the connections from the plan - capabilities2_xml_parser::extract_connections(plan, connection_map); - - RCLCPP_INFO(this->get_logger(), "Connection extraction successful"); - - // estasblish the bond with the server - if (!establish_bond()) - { - RCLCPP_INFO(this->get_logger(), "Establishing bond failed"); - - // TODO: improve with error codes - result->success = false; - server_goal_handle->canceled(result); - - RCLCPP_INFO(this->get_logger(), "Server Execution Cancelled"); - } - - RCLCPP_INFO(this->get_logger(), "Bond establishment successful"); - - // start all runners and interfaces that the connections depend on - for (const auto& [key, value] : connection_map) - { - RCLCPP_INFO(this->get_logger(), "Starting capability of Node : %i", key); - - // start the capability with bond id. - if (use_capability(value.source.runner, value.source.provider, bond_id)) - { - // capability started succeesfully - RCLCPP_INFO(get_logger(), "Capability started: %s", value.source.runner.c_str()); - } - else - { - std::string parameter_string; - capabilities2_xml_parser::convert_to_string(value.source.parameters, parameter_string); - - // capability failed. so add it to failed connections to be sent to the action client - result->failed_elements.push_back(parameter_string); - RCLCPP_ERROR(get_logger(), "Capability failed: %s", parameter_string.c_str()); - } - } - - RCLCPP_INFO(this->get_logger(), "All capability starting successful"); - - // check if there are any failed elements of the plan - if (result->failed_elements.size() > 0) - { - // there are failed connections. so cancel the action server process. and let the action - // client know that about the failed connection - server_goal_handle->canceled(result); - - // free the capabilites that were started since action execution failed - for (const auto& [key, value] : connection_map) - { - free_capability(value.source.runner, bond_id); - RCLCPP_ERROR(get_logger(), "Capability freed due to failure: %s", value.source.runner.c_str()); - } - } - - // configuring the capabilities - if (!configure_capabilities()) - { - RCLCPP_INFO(this->get_logger(), "Configuring capabilities failed"); - - // TODO: improve with error codes - result->success = false; - server_goal_handle->canceled(result); - - RCLCPP_INFO(this->get_logger(), "Server Execution Cancelled"); - } - - RCLCPP_INFO(this->get_logger(), "Capability configuration successful"); - - // triggering the first node to start the fabric - if (!trigger_first_node()) - { - RCLCPP_INFO(this->get_logger(), "Triggering first capability failed"); + auto result_future = trigger_capability_client_->async_send_request( + request_trigger, [this, goal_handle, feedback](TriggerCapabilityClient::SharedFuture future) { + if (!future.valid()) + { + auto result = std::make_shared(); + result->success = false; + result->message = "Failed to trigger capability " + connection_map[0].source.runner; + goal_handle->abort(result); - // TODO: improve with error codes - result->success = false; - server_goal_handle->canceled(result); + RCLCPP_ERROR(this->get_logger(), result->message.c_str()); + return; + } - RCLCPP_INFO(this->get_logger(), "Server Execution Cancelled"); - } + auto response = future.get(); + feedback->progress = "Successfully triggered capability " + connection_map[0].source.runner; + goal_handle->publish_feedback(feedback); + RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); + }); } private: @@ -641,6 +793,15 @@ class CapabilitiesFabric : public rclcpp::Node /** flag to select loading from file or accepting via action server */ bool read_file; + int expected_providers_; + int completed_providers_; + + int expected_capabilities_; + int completed_capabilities_; + + int expected_configurations_; + int completed_configurations_; + /** Bond ID between capabilities server and this client */ std::string bond_id; @@ -650,9 +811,6 @@ class CapabilitiesFabric : public rclcpp::Node /** vector of connections */ std::map connection_map; - /** Execution Thread */ - std::shared_ptr fabric_thread; - /** Interface List */ std::vector interface_list; @@ -666,32 +824,32 @@ class CapabilitiesFabric : public rclcpp::Node std::vector rejected_list; /** action server that exposes executor*/ - std::shared_ptr> planner_server_; + std::shared_ptr> planner_server_; /** action server goal handle*/ - std::shared_ptr> goal_handle; + std::shared_ptr goal_handle; /** Get interfaces from capabilities server */ - rclcpp::Client::SharedPtr get_interfaces_client_; + GetInterfacesClient::SharedPtr get_interfaces_client_; /** Get semantic interfaces from capabilities server */ - rclcpp::Client::SharedPtr get_sem_interf_client_; + GetSemanticInterfacesClient::SharedPtr get_sem_interf_client_; /** Get providers from capabilities server */ - rclcpp::Client::SharedPtr get_providers_client_; + GetProvidersClient::SharedPtr get_providers_client_; /** establish bond */ - rclcpp::Client::SharedPtr establish_bond_client_; + EstablishBondClient::SharedPtr establish_bond_client_; /** use an selected capability */ - rclcpp::Client::SharedPtr use_capability_client_; + UseCapabilityClient::SharedPtr use_capability_client_; /** free an selected capability */ - rclcpp::Client::SharedPtr free_capability_client_; + FreeCapabilityClient::SharedPtr free_capability_client_; /** configure an selected capability */ - rclcpp::Client::SharedPtr configure_capability_client_; + ConfigureCapabilityClient::SharedPtr configure_capability_client_; /** trigger an selected capability */ - rclcpp::Client::SharedPtr trigger_capability_client_; + TriggerCapabilityClient::SharedPtr trigger_capability_client_; }; \ No newline at end of file diff --git a/capabilities2_fabric/include/capabilities2_executor/capabilities_file_parser.hpp b/capabilities2_fabric/include/capabilities2_executor/capabilities_file_parser.hpp index 4460047..c576712 100644 --- a/capabilities2_fabric/include/capabilities2_executor/capabilities_file_parser.hpp +++ b/capabilities2_fabric/include/capabilities2_executor/capabilities_file_parser.hpp @@ -25,11 +25,20 @@ class CapabilitiesFileParser : public rclcpp::Node public: CapabilitiesFileParser() : Node("Capabilities2_File_Parser") { - declare_parameter("plan_file_path", "install/capabilities2_executor/share/capabilities2_executor/plans/default.xml"); + declare_parameter("plan_file_path", "install/capabilities2_fabric/share/capabilities2_fabric/plans/default.xml"); plan_file_path = get_parameter("plan_file_path").as_string(); + } - this->client_ptr_ = rclcpp_action::create_client(this, "~/capabilities_fabric"); + /** + * @brief Initializer function + * + */ + void initialize() + { + // Create the action client for capabilities_fabric after the node is fully constructed + this->client_ptr_ = rclcpp_action::create_client(shared_from_this(), "/capabilities_fabric"); + // Set up a timer to call send_goal after a short delay this->timer_ = this->create_wall_timer(std::chrono::milliseconds(500), std::bind(&CapabilitiesFileParser::send_goal, this)); } @@ -43,22 +52,28 @@ class CapabilitiesFileParser : public rclcpp::Node // check if the file loading failed if (xml_status != tinyxml2::XMLError::XML_SUCCESS) { - RCLCPP_INFO(this->get_logger(), "Loading the file from path : %s failed", plan_file_path.c_str()); + RCLCPP_ERROR(this->get_logger(), "Error loading plan: %s, Error: %s", plan_file_path.c_str(), document.ErrorName()); rclcpp::shutdown(); } - RCLCPP_INFO(this->get_logger(), "Sucessfully loaded the file from path : %s", plan_file_path.c_str()); + RCLCPP_INFO(this->get_logger(), "Plan loaded from : %s", plan_file_path.c_str()); - if (!this->client_ptr_->wait_for_action_server()) + if (!this->client_ptr_->wait_for_action_server(std::chrono::seconds(5))) { RCLCPP_ERROR(this->get_logger(), "Action server not available after waiting"); rclcpp::shutdown(); + return; } + RCLCPP_INFO(this->get_logger(), "Sucessfully connected to the capabilities_fabric action server"); + auto goal_msg = capabilities2_msgs::action::Plan::Goal(); - goal_msg.plan = capabilities2_xml_parser::convert_to_string(document); - RCLCPP_INFO(this->get_logger(), "Sending goal"); + capabilities2_xml_parser::convert_to_string(document, goal_msg.plan); + + RCLCPP_INFO(this->get_logger(), "Following plan was read:\n\n%s ", goal_msg.plan.c_str()); + + RCLCPP_INFO(this->get_logger(), "Sending goal to the capabilities_fabric action server"); auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); diff --git a/capabilities2_fabric/include/capabilities2_executor/capabilities_xml_parser.hpp b/capabilities2_fabric/include/capabilities2_executor/capabilities_xml_parser.hpp index 81308c2..28f2621 100644 --- a/capabilities2_fabric/include/capabilities2_executor/capabilities_xml_parser.hpp +++ b/capabilities2_fabric/include/capabilities2_executor/capabilities_xml_parser.hpp @@ -90,12 +90,11 @@ bool convert_to_string(tinyxml2::XMLElement* element, std::string& paramters) * * @return std::string converted document */ -std::string convert_to_string(tinyxml2::XMLDocument& document) +void convert_to_string(tinyxml2::XMLDocument& document_xml, std::string& document_string) { tinyxml2::XMLPrinter printer; - document.Accept(&printer); - std::string value = printer.CStr(); - return value; + document_xml.Print(&printer); + document_string = printer.CStr(); } /** diff --git a/capabilities2_fabric/launch/fabric.launch.py b/capabilities2_fabric/launch/fabric.launch.py index d3a4ccc..243598c 100644 --- a/capabilities2_fabric/launch/fabric.launch.py +++ b/capabilities2_fabric/launch/fabric.launch.py @@ -21,7 +21,8 @@ def generate_launch_description(): package='capabilities2_fabric', namespace='', executable='capabilities2_fabric', - name='capabilities2_fabric' + name='capabilities2_fabric', + output='screen' ) executor_file = Node( @@ -29,7 +30,8 @@ def generate_launch_description(): namespace='', executable='capabilities2_file_parser', name='capabilities2_file_parser', - parameters=[executor_config] + parameters=[executor_config], + output='screen' ) # return diff --git a/capabilities2_fabric/src/capabilities_fabric.cpp b/capabilities2_fabric/src/capabilities_fabric.cpp index 19e8bc9..006bcc2 100644 --- a/capabilities2_fabric/src/capabilities_fabric.cpp +++ b/capabilities2_fabric/src/capabilities_fabric.cpp @@ -1,9 +1,16 @@ #include -int main(int argc, char * argv[]) +int main(int argc, char* argv[]) { rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); + + // Create the node instance + auto node = std::make_shared(); + + // Initialize the node components after construction + node->initialize(); + rclcpp::spin(node); + rclcpp::shutdown(); return 0; -} +} \ No newline at end of file diff --git a/capabilities2_fabric/src/capabilities_file_parser.cpp b/capabilities2_fabric/src/capabilities_file_parser.cpp index 00c2410..aa0d9b0 100644 --- a/capabilities2_fabric/src/capabilities_file_parser.cpp +++ b/capabilities2_fabric/src/capabilities_file_parser.cpp @@ -1,9 +1,15 @@ #include -int main(int argc, char * argv[]) +int main(int argc, char* argv[]) { rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); + + auto parser_node = std::make_shared(); + + parser_node->initialize(); // Call initialize after construction + + rclcpp::spin(parser_node); rclcpp::shutdown(); + return 0; } diff --git a/capabilities2_msgs/action/Plan.action b/capabilities2_msgs/action/Plan.action index ad84fa9..ab827c1 100644 --- a/capabilities2_msgs/action/Plan.action +++ b/capabilities2_msgs/action/Plan.action @@ -6,7 +6,9 @@ string plan # result std_msgs/Header header bool success +string message string[] failed_elements --- # feedback std_msgs/Header header +string progress diff --git a/capabilities2_runner/CMakeLists.txt b/capabilities2_runner/CMakeLists.txt index 02d553d..8d885d0 100644 --- a/capabilities2_runner/CMakeLists.txt +++ b/capabilities2_runner/CMakeLists.txt @@ -15,11 +15,16 @@ find_package(rclcpp REQUIRED) find_package(rclcpp_action REQUIRED) find_package(pluginlib REQUIRED) find_package(capabilities2_msgs REQUIRED) -find_package(PkgConfig) -pkg_check_modules(TINYXML2 tinyxml2 REQUIRED) + +# Locate the static version of tinyxml2 +find_library(TINYXML2_LIB NAMES tinyxml2 PATHS /usr/local/lib NO_DEFAULT_PATH) + +if(NOT TINYXML2_LIB) + message(FATAL_ERROR "tinyxml2 library not found. Make sure it's installed as a static library.") +endif() include_directories(include -${TINYXML2_INCLUDE_DIRS} + /usr/local/include ) add_library(${PROJECT_NAME} SHARED @@ -29,7 +34,7 @@ add_library(${PROJECT_NAME} SHARED ) target_link_libraries(${PROJECT_NAME} - ${TINYXML2_LIBRARIES} + ${TINYXML2_LIB} ) ament_target_dependencies(${PROJECT_NAME} diff --git a/capabilities2_runner_audio/CMakeLists.txt b/capabilities2_runner_audio/CMakeLists.txt index 15ba918..f6cc0ce 100644 --- a/capabilities2_runner_audio/CMakeLists.txt +++ b/capabilities2_runner_audio/CMakeLists.txt @@ -18,11 +18,16 @@ find_package(pluginlib REQUIRED) find_package(hri_audio_msgs REQUIRED) find_package(capabilities2_msgs REQUIRED) find_package(capabilities2_runner REQUIRED) -find_package(PkgConfig) -pkg_check_modules(TINYXML2 tinyxml2 REQUIRED) + +# Locate the static version of tinyxml2 +find_library(TINYXML2_LIB NAMES tinyxml2 PATHS /usr/local/lib NO_DEFAULT_PATH) + +if(NOT TINYXML2_LIB) + message(FATAL_ERROR "tinyxml2 library not found. Make sure it's installed as a static library.") +endif() include_directories(include -${TINYXML2_INCLUDE_DIRS} + /usr/local/include ) add_library(${PROJECT_NAME} SHARED @@ -30,7 +35,7 @@ add_library(${PROJECT_NAME} SHARED ) target_link_libraries(${PROJECT_NAME} - ${TINYXML2_LIBRARIES} + ${TINYXML2_LIB} ) ament_target_dependencies(${PROJECT_NAME} diff --git a/capabilities2_runner_bt/CMakeLists.txt b/capabilities2_runner_bt/CMakeLists.txt index aae09e2..092a483 100644 --- a/capabilities2_runner_bt/CMakeLists.txt +++ b/capabilities2_runner_bt/CMakeLists.txt @@ -17,11 +17,16 @@ find_package(pluginlib REQUIRED) find_package(capabilities2_msgs REQUIRED) find_package(capabilities2_runner REQUIRED) find_package(behaviortree_cpp REQUIRED) -find_package(PkgConfig) -pkg_check_modules(TINYXML2 tinyxml2 REQUIRED) + +# Locate the static version of tinyxml2 +find_library(TINYXML2_LIB NAMES tinyxml2 PATHS /usr/local/lib NO_DEFAULT_PATH) + +if(NOT TINYXML2_LIB) + message(FATAL_ERROR "tinyxml2 library not found. Make sure it's installed as a static library.") +endif() include_directories(include -${TINYXML2_INCLUDE_DIRS} + /usr/local/include ) add_library(${PROJECT_NAME} SHARED @@ -29,7 +34,7 @@ add_library(${PROJECT_NAME} SHARED ) target_link_libraries(${PROJECT_NAME} - ${TINYXML2_LIBRARIES} + ${TINYXML2_LIB} ) ament_target_dependencies(${PROJECT_NAME} diff --git a/capabilities2_runner_nav2/CMakeLists.txt b/capabilities2_runner_nav2/CMakeLists.txt index 3057740..df75cbc 100644 --- a/capabilities2_runner_nav2/CMakeLists.txt +++ b/capabilities2_runner_nav2/CMakeLists.txt @@ -18,11 +18,16 @@ find_package(nav2_msgs REQUIRED) find_package(geometry_msgs REQUIRED) find_package(capabilities2_msgs REQUIRED) find_package(capabilities2_runner REQUIRED) -find_package(PkgConfig) -pkg_check_modules(TINYXML2 tinyxml2 REQUIRED) + +# Locate the static version of tinyxml2 +find_library(TINYXML2_LIB NAMES tinyxml2 PATHS /usr/local/lib NO_DEFAULT_PATH) + +if(NOT TINYXML2_LIB) + message(FATAL_ERROR "tinyxml2 library not found. Make sure it's installed as a static library.") +endif() include_directories(include -${TINYXML2_INCLUDE_DIRS} + /usr/local/include ) add_library(${PROJECT_NAME} SHARED @@ -30,7 +35,7 @@ add_library(${PROJECT_NAME} SHARED ) target_link_libraries(${PROJECT_NAME} - ${TINYXML2_LIBRARIES} + ${TINYXML2_LIB} ) ament_target_dependencies(${PROJECT_NAME} diff --git a/capabilities2_runner_prompt/CMakeLists.txt b/capabilities2_runner_prompt/CMakeLists.txt index c82454a..22295df 100644 --- a/capabilities2_runner_prompt/CMakeLists.txt +++ b/capabilities2_runner_prompt/CMakeLists.txt @@ -17,11 +17,16 @@ find_package(pluginlib REQUIRED) find_package(prompt_msgs REQUIRED) find_package(capabilities2_msgs REQUIRED) find_package(capabilities2_runner REQUIRED) -find_package(PkgConfig) -pkg_check_modules(TINYXML2 tinyxml2 REQUIRED) + +# Locate the static version of tinyxml2 +find_library(TINYXML2_LIB NAMES tinyxml2 PATHS /usr/local/lib NO_DEFAULT_PATH) + +if(NOT TINYXML2_LIB) + message(FATAL_ERROR "tinyxml2 library not found. Make sure it's installed as a static library.") +endif() include_directories(include -${TINYXML2_INCLUDE_DIRS} + /usr/local/include ) add_library(${PROJECT_NAME} SHARED @@ -29,7 +34,7 @@ add_library(${PROJECT_NAME} SHARED ) target_link_libraries(${PROJECT_NAME} - ${TINYXML2_LIBRARIES} + ${TINYXML2_LIB} ) ament_target_dependencies(${PROJECT_NAME} diff --git a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_response_runner.hpp b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_response_runner.hpp index 38cb6e2..48e4c56 100644 --- a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_response_runner.hpp +++ b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_response_runner.hpp @@ -33,7 +33,7 @@ class PromptPlanResponseRunner : public ActionRunner Date: Wed, 13 Nov 2024 17:29:50 +1100 Subject: [PATCH 027/133] updated provider names and ffixes to fabric --- .../capabilities_fabric.hpp | 61 +++++++++----- .../capabilities_xml_parser.hpp | 80 ++++++++++++------- capabilities2_fabric/plans/navigation_1.xml | 2 +- .../providers/audio/ListenerLaunchRunner.yaml | 2 +- .../providers/audio/ListenerRunner.yaml | 6 +- .../providers/audio/SpeakerLaunchRunner.yaml | 2 +- .../providers/audio/SpeakerRunner.yaml | 6 +- .../providers/nav2/Nav2LaunchRunner.yaml | 2 +- .../providers/nav2/OccupancyGridRunner.yaml | 6 +- .../providers/nav2/RobotPoseRunner.yaml | 6 +- .../providers/nav2/WaypointRunner.yaml | 6 +- .../providers/prompt/PromptLaunchRunner.yaml | 2 +- .../prompt/PromptOccupancyRunner.yaml | 6 +- .../prompt/PromptPlanRequestRunner.yaml | 6 +- .../prompt/PromptPlanResponseRunner.yaml | 6 +- .../providers/prompt/PromptPoseRunner.yaml | 6 +- .../providers/prompt/PromptTextRunner.yaml | 6 +- 17 files changed, 127 insertions(+), 84 deletions(-) diff --git a/capabilities2_fabric/include/capabilities2_executor/capabilities_fabric.hpp b/capabilities2_fabric/include/capabilities2_executor/capabilities_fabric.hpp index 25c76ca..e000995 100644 --- a/capabilities2_fabric/include/capabilities2_executor/capabilities_fabric.hpp +++ b/capabilities2_fabric/include/capabilities2_executor/capabilities_fabric.hpp @@ -215,10 +215,10 @@ class CapabilitiesFabric : public rclcpp::Node void getInterfaces(const std::shared_ptr goal_handle) { auto feedback = std::make_shared(); - feedback->progress = "Requesting Interface information..."; + feedback->progress = "Requesting Interface information"; goal_handle->publish_feedback(feedback); - RCLCPP_INFO(this->get_logger(), "Requesting Interface information..."); + RCLCPP_INFO(this->get_logger(), "Requesting Interface information"); auto request_interface = std::make_shared(); @@ -247,8 +247,6 @@ class CapabilitiesFabric : public rclcpp::Node { getSemanticInterfaces(interface, goal_handle); } - - RCLCPP_INFO(this->get_logger(), "Interface retreival successful"); }); } @@ -283,12 +281,15 @@ class CapabilitiesFabric : public rclcpp::Node } auto response = future.get(); - RCLCPP_INFO(this->get_logger(), "Received Semantic Interface information"); if (response->semantic_interfaces.size() > 0) { for (const auto& semantic_interface : response->semantic_interfaces) { + feedback->progress = "Received Semantic Interface information for " + interface + " as " + semantic_interface; + goal_handle->publish_feedback(feedback); + RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); + expected_providers_++; interface_list.push_back(semantic_interface); getProvider(semantic_interface, goal_handle, true); @@ -297,6 +298,10 @@ class CapabilitiesFabric : public rclcpp::Node // if no semantic interfaces are availble for a given interface, add the interface instead else { + feedback->progress = "No Semantic Interface information for " + interface; + goal_handle->publish_feedback(feedback); + RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); + expected_providers_++; interface_list.push_back(interface); getProvider(interface, goal_handle, false); @@ -356,7 +361,7 @@ class CapabilitiesFabric : public rclcpp::Node if (completed_providers_ == expected_providers_) { auto feedback = std::make_shared(); - feedback->progress = "All requested provider data recieved"; + feedback->progress = "All requested interface, semantic interface and provider data recieved"; goal_handle->publish_feedback(feedback); RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); @@ -377,7 +382,7 @@ class CapabilitiesFabric : public rclcpp::Node RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); // verify the plan - if (!verify_plan()) + if (!verify_plan(goal_handle)) { feedback->progress = "Plan verification failed"; goal_handle->publish_feedback(feedback); @@ -406,6 +411,7 @@ class CapabilitiesFabric : public rclcpp::Node result->success = false; result->message = "Plan verification failed."; goal_handle->abort(result); + RCLCPP_ERROR(this->get_logger(), "Server Execution Cancelled"); } RCLCPP_ERROR(this->get_logger(), "Server Execution Cancelled"); @@ -438,8 +444,11 @@ class CapabilitiesFabric : public rclcpp::Node * * @return `true` if interface retreival is successful,`false` otherwise */ - bool verify_plan() + bool verify_plan(const std::shared_ptr goal_handle) { + auto feedback = std::make_shared(); + auto result = std::make_shared(); + // intialize a vector to accomodate elements from both std::vector tag_list(interface_list.size() + control_tag_list.size()); std::merge(interface_list.begin(), interface_list.end(), control_tag_list.begin(), control_tag_list.end(), tag_list.begin()); @@ -447,20 +456,37 @@ class CapabilitiesFabric : public rclcpp::Node // verify whether document got 'plan' tags if (!capabilities2_xml_parser::check_plan_tag(document)) { - RCLCPP_INFO(this->get_logger(), "Execution plan is not compatible. Please recheck and update"); + result->success = false; + result->message = "Execution plan is not compatible. Please recheck and update"; + goal_handle->abort(result); + RCLCPP_ERROR(this->get_logger(), result->message.c_str()); return false; } + feedback->progress = "Plan tag checking successful"; + goal_handle->publish_feedback(feedback); + RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); + // extract the components within the 'plan' tags tinyxml2::XMLElement* plan = capabilities2_xml_parser::get_plan(document); + feedback->progress = "Plan extraction complete"; + goal_handle->publish_feedback(feedback); + RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); + // verify whether the plan is valid - if (!capabilities2_xml_parser::check_tags(plan, interface_list, providers_list, control_tag_list, rejected_list)) + if (!capabilities2_xml_parser::check_tags(this->get_logger(), plan, interface_list, providers_list, control_tag_list, rejected_list)) { - RCLCPP_INFO(this->get_logger(), "Execution plan is faulty. Please recheck and update"); + result->success = false; + result->message = "Execution plan is faulty. Please recheck and update"; + goal_handle->abort(result); + RCLCPP_ERROR(this->get_logger(), result->message.c_str()); return false; } + feedback->progress = "Checking tags successful"; + goal_handle->publish_feedback(feedback); + RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); return true; } @@ -480,8 +506,8 @@ class CapabilitiesFabric : public rclcpp::Node auto request_bond = std::make_shared(); // send the request - auto result_future = establish_bond_client_->async_send_request( - request_bond, [this, goal_handle, feedback](EstablishBondClient::SharedFuture future) { + auto result_future = + establish_bond_client_->async_send_request(request_bond, [this, goal_handle, feedback](EstablishBondClient::SharedFuture future) { if (!future.valid()) { RCLCPP_ERROR(this->get_logger(), "Failed to receive the bond id"); @@ -625,7 +651,7 @@ class CapabilitiesFabric : public rclcpp::Node void configure_capabilities(const std::shared_ptr goal_handle) { auto feedback = std::make_shared(); - + for (const auto& [key, value] : connection_map) { auto request_configure = std::make_shared(); @@ -713,8 +739,7 @@ class CapabilitiesFabric : public rclcpp::Node // send the request auto result_future = configure_capability_client_->async_send_request( - request_configure, - [this, goal_handle, value, feedback](ConfigureCapabilityClient::SharedFuture future) { + request_configure, [this, goal_handle, value, feedback](ConfigureCapabilityClient::SharedFuture future) { if (!future.valid()) { auto result = std::make_shared(); @@ -766,8 +791,8 @@ class CapabilitiesFabric : public rclcpp::Node request_trigger->parameters = parameter_string; // send the request - auto result_future = trigger_capability_client_->async_send_request( - request_trigger, [this, goal_handle, feedback](TriggerCapabilityClient::SharedFuture future) { + auto result_future = + trigger_capability_client_->async_send_request(request_trigger, [this, goal_handle, feedback](TriggerCapabilityClient::SharedFuture future) { if (!future.valid()) { auto result = std::make_shared(); diff --git a/capabilities2_fabric/include/capabilities2_executor/capabilities_xml_parser.hpp b/capabilities2_fabric/include/capabilities2_executor/capabilities_xml_parser.hpp index 28f2621..a9287be 100644 --- a/capabilities2_fabric/include/capabilities2_executor/capabilities_xml_parser.hpp +++ b/capabilities2_fabric/include/capabilities2_executor/capabilities_xml_parser.hpp @@ -1,6 +1,7 @@ #include #include #include +#include #include namespace capabilities2_xml_parser @@ -14,7 +15,7 @@ namespace capabilities2_xml_parser */ tinyxml2::XMLElement* get_plan(tinyxml2::XMLDocument& document) { - return document.FirstChildElement("Plan"); + return document.FirstChildElement("Plan")->FirstChildElement(); } /** @@ -109,21 +110,47 @@ void convert_to_string(tinyxml2::XMLDocument& document_xml, std::string& documen * * @return `true` if element valid and supported and `false` otherwise */ -bool check_tags(tinyxml2::XMLElement* element, std::vector& events, std::vector& providers, +bool check_tags(rclcpp::Logger logger, tinyxml2::XMLElement* element, std::vector& events, std::vector& providers, std::vector& control, std::vector& rejected) { - const char** name; - const char** provider; + const char* name; + const char* provider; - element->QueryStringAttribute("name", name); - element->QueryStringAttribute("provider", provider); + std::string parameter_string; + convert_to_string(element, parameter_string); + + element->QueryStringAttribute("name", &name); + element->QueryStringAttribute("provider", &provider); + + std::string nametag; + std::string providertag; + + // // Check and retrieve "name" and "provider" attributes safely + // if (element->QueryStringAttribute("name", &name) != tinyxml2::XML_SUCCESS || name == nullptr) + // { + // RCLCPP_ERROR(logger, "Missing 'name' attribute in XML element : %s", parameter_string.c_str()); + // rejected.push_back(parameter_string); + // return false; + // } + + // if (element->QueryStringAttribute("provider", &provider) != tinyxml2::XML_SUCCESS || provider == nullptr) + // { + // RCLCPP_ERROR(logger, "Missing 'provider' attribute in XML element : %s", parameter_string.c_str()); + // rejected.push_back(parameter_string); + // return false; + // } std::string typetag(element->Name()); - std::string nametag(*name); - std::string providertag(*provider); - std::string parameter_string; - convert_to_string(element, parameter_string); + if (name) + nametag = name; + else + nametag = ""; + + if (provider) + providertag = provider; + else + providertag = ""; bool hasChildren = !element->NoChildren(); bool hasSiblings = !capabilities2_xml_parser::isLastElement(element); @@ -134,43 +161,34 @@ bool check_tags(tinyxml2::XMLElement* element, std::vector& events, if (typetag == "Control") { - if (foundInControl and hasChildren) - returnValue = returnValue and capabilities2_xml_parser::check_tags(element->FirstChildElement(), events, providers, control, rejected); - - if (foundInControl and hasSiblings) - returnValue = returnValue and capabilities2_xml_parser::check_tags(element->NextSiblingElement(), events, providers, control, rejected); - - if (foundInControl and !hasSiblings) - returnValue = returnValue; - if (!foundInControl) { + RCLCPP_ERROR(logger, "Control tag '%s' not available in the valid list", nametag.c_str()); rejected.push_back(parameter_string); return false; } + + if (hasChildren) + returnValue &= capabilities2_xml_parser::check_tags(logger, element->FirstChildElement(), events, providers, control, rejected); + + if (hasSiblings) + returnValue &= capabilities2_xml_parser::check_tags(logger, element->NextSiblingElement(), events, providers, control, rejected); } else if (typetag == "Event") { - if (foundInEvents and foundInProviders and hasSiblings) - returnValue = returnValue and capabilities2_xml_parser::check_tags(element->NextSiblingElement(), events, providers, control, rejected); - - if (foundInEvents and foundInProviders and !hasSiblings) - returnValue = returnValue; - - if (!foundInEvents) + if (!foundInEvents || !foundInProviders) { + RCLCPP_ERROR(logger, "Event tag name '%s' or provider '%s' not available in the valid list", nametag.c_str(), providertag.c_str()); rejected.push_back(parameter_string); return false; } - if (!foundInProviders) - { - rejected.push_back(parameter_string); - return false; - } + if (hasSiblings) + returnValue &= capabilities2_xml_parser::check_tags(logger, element->NextSiblingElement(), events, providers, control, rejected); } else { + RCLCPP_ERROR(logger, "xml element is not valid", parameter_string.c_str()); rejected.push_back(parameter_string); return false; } diff --git a/capabilities2_fabric/plans/navigation_1.xml b/capabilities2_fabric/plans/navigation_1.xml index e02793a..ac16237 100644 --- a/capabilities2_fabric/plans/navigation_1.xml +++ b/capabilities2_fabric/plans/navigation_1.xml @@ -1,6 +1,6 @@ - + \ No newline at end of file diff --git a/std_capabilities2/providers/audio/ListenerLaunchRunner.yaml b/std_capabilities2/providers/audio/ListenerLaunchRunner.yaml index 6321911..8572453 100644 --- a/std_capabilities2/providers/audio/ListenerLaunchRunner.yaml +++ b/std_capabilities2/providers/audio/ListenerLaunchRunner.yaml @@ -5,5 +5,5 @@ name: ListenerLaunchRunner spec_type: provider spec_version: 1.1 description: "The capability provider for the audio stack" -implements: capabilities2_runner_audio/ListenerLaunchRunner +implements: std_capabilities2/ListenerLaunchRunner runner: launch/audio/listen.launch.py diff --git a/std_capabilities2/providers/audio/ListenerRunner.yaml b/std_capabilities2/providers/audio/ListenerRunner.yaml index d2e201e..ec94941 100644 --- a/std_capabilities2/providers/audio/ListenerRunner.yaml +++ b/std_capabilities2/providers/audio/ListenerRunner.yaml @@ -5,8 +5,8 @@ name: ListenerRunner spec_type: provider spec_version: 1.1 description: The capability provider for the speech_to_text interface -implements: capabilities2_runner_audio/ListenerRunner +implements: std_capabilities2/ListenerRunner runner: capabilities2_runner::ListenerRunner depends_on: - "capabilities2_runner_audio/ListenerLaunchRunner": - provider: "capabilities2_runner_audio/ListenerLaunchRunner" \ No newline at end of file + "std_capabilities2/ListenerLaunchRunner": + provider: "std_capabilities2/ListenerLaunchRunner" \ No newline at end of file diff --git a/std_capabilities2/providers/audio/SpeakerLaunchRunner.yaml b/std_capabilities2/providers/audio/SpeakerLaunchRunner.yaml index 0b2b7db..1f250db 100644 --- a/std_capabilities2/providers/audio/SpeakerLaunchRunner.yaml +++ b/std_capabilities2/providers/audio/SpeakerLaunchRunner.yaml @@ -5,5 +5,5 @@ name: SpeakerLaunchRunner spec_type: provider spec_version: 1.1 description: "The capability provider for the audio stack" -implements: capabilities2_runner_audio/SpeakerLaunchRunner +implements: std_capabilities2/SpeakerLaunchRunner runner: launch/audio/speak.launch.py \ No newline at end of file diff --git a/std_capabilities2/providers/audio/SpeakerRunner.yaml b/std_capabilities2/providers/audio/SpeakerRunner.yaml index c5a52f2..ecc5656 100644 --- a/std_capabilities2/providers/audio/SpeakerRunner.yaml +++ b/std_capabilities2/providers/audio/SpeakerRunner.yaml @@ -5,8 +5,8 @@ name: SpeakerRunner spec_type: provider spec_version: 1.1 description: The capability provider for the text_to_speech interface -implements: capabilities2_runner_nav2/SpeakerRunner +implements: std_capabilities2/SpeakerRunner runner: capabilities2_runner::SpeakerRunner depends_on: - "capabilities2_runner_audio/SpeakerLaunchRunner": - provider: "capabilities2_runner_audio/SpeakerLaunchRunner" \ No newline at end of file + "std_capabilities2/SpeakerLaunchRunner": + provider: "std_capabilities2/SpeakerLaunchRunner" \ No newline at end of file diff --git a/std_capabilities2/providers/nav2/Nav2LaunchRunner.yaml b/std_capabilities2/providers/nav2/Nav2LaunchRunner.yaml index 3a297e1..4a02375 100644 --- a/std_capabilities2/providers/nav2/Nav2LaunchRunner.yaml +++ b/std_capabilities2/providers/nav2/Nav2LaunchRunner.yaml @@ -5,5 +5,5 @@ name: Nav2LaunchRunner spec_type: provider spec_version: 1.1 description: "The capability provider for the nav2 stack" -implements: capabilities2_runner_nav2/Nav2LaunchRunner +implements: std_capabilities2/Nav2LaunchRunner runner: launch/nav2/nav2stack.launch.py diff --git a/std_capabilities2/providers/nav2/OccupancyGridRunner.yaml b/std_capabilities2/providers/nav2/OccupancyGridRunner.yaml index 81b5542..f9e6dae 100644 --- a/std_capabilities2/providers/nav2/OccupancyGridRunner.yaml +++ b/std_capabilities2/providers/nav2/OccupancyGridRunner.yaml @@ -5,8 +5,8 @@ name: OccupancyGridRunner spec_type: provider spec_version: 1.1 description: "The capability provider for the robot's occupancy grid map interface" -implements: capabilities2_runner_nav2/OccupancyGridRunner +implements: std_capabilities2/OccupancyGridRunner runner: capabilities2_runner::OccupancyGridRunner depends_on: - "capabilities2_runner_nav2/Nav2LaunchRunner": - provider: "capabilities2_runner_nav2/Nav2LaunchRunner" \ No newline at end of file + "std_capabilities2/Nav2LaunchRunner": + provider: "std_capabilities2/Nav2LaunchRunner" \ No newline at end of file diff --git a/std_capabilities2/providers/nav2/RobotPoseRunner.yaml b/std_capabilities2/providers/nav2/RobotPoseRunner.yaml index d0572ed..7f17dbc 100644 --- a/std_capabilities2/providers/nav2/RobotPoseRunner.yaml +++ b/std_capabilities2/providers/nav2/RobotPoseRunner.yaml @@ -5,8 +5,8 @@ name: RobotPoseRunner spec_type: provider spec_version: 1.1 description: "The capability provider for the robot's odometry interface" -implements: capabilities2_runner_nav2/RobotPoseRunner +implements: std_capabilities2/RobotPoseRunner runner: capabilities2_runner::RobotPoseRunner depends_on: - "capabilities2_runner_nav2/Nav2LaunchRunner": - provider: "capabilities2_runner_nav2/Nav2LaunchRunner" \ No newline at end of file + "std_capabilities2/Nav2LaunchRunner": + provider: "std_capabilities2/Nav2LaunchRunner" \ No newline at end of file diff --git a/std_capabilities2/providers/nav2/WaypointRunner.yaml b/std_capabilities2/providers/nav2/WaypointRunner.yaml index da5770b..774229e 100644 --- a/std_capabilities2/providers/nav2/WaypointRunner.yaml +++ b/std_capabilities2/providers/nav2/WaypointRunner.yaml @@ -5,8 +5,8 @@ name: WaypointRunner spec_type: provider spec_version: 1.1 description: "The capability provider for the follow_waypoint interface" -implements: capabilities2_runner_nav2/WaypointRunner +implements: std_capabilities2/WaypointRunner runner: capabilities2_runner::WayPointRunner depends_on: - "capabilities2_runner_nav2/Nav2LaunchRunner": - provider: "capabilities2_runner_nav2/Nav2LaunchRunner" \ No newline at end of file + "std_capabilities2/Nav2LaunchRunner": + provider: "std_capabilities2/Nav2LaunchRunner" \ No newline at end of file diff --git a/std_capabilities2/providers/prompt/PromptLaunchRunner.yaml b/std_capabilities2/providers/prompt/PromptLaunchRunner.yaml index 0fd1050..94b7dec 100644 --- a/std_capabilities2/providers/prompt/PromptLaunchRunner.yaml +++ b/std_capabilities2/providers/prompt/PromptLaunchRunner.yaml @@ -5,5 +5,5 @@ name: PromptLaunchRunner spec_type: provider spec_version: 1.1 description: "The capability provider for the prompt tools" -implements: capabilities2_runner_prompt/PromptLaunchRunner +implements: std_capabilities2/PromptLaunchRunner runner: launch/prompt/prompttool.launch.py diff --git a/std_capabilities2/providers/prompt/PromptOccupancyRunner.yaml b/std_capabilities2/providers/prompt/PromptOccupancyRunner.yaml index 6d3f206..25f1f63 100644 --- a/std_capabilities2/providers/prompt/PromptOccupancyRunner.yaml +++ b/std_capabilities2/providers/prompt/PromptOccupancyRunner.yaml @@ -5,8 +5,8 @@ name: PromptOccupancyRunner spec_type: provider spec_version: 1.1 description: "The capability provider for the prompting interface from prompt tools. This can be used for prompting the occupancy map around the robot." -implements: capabilities2_runner_prompt/PromptOccupancyRunner +implements: std_capabilities2/PromptOccupancyRunner runner: capabilities2_runner::PromptOccupancyRunner depends_on: - "capabilities2_runner_prompt/PromptLaunchRunner": - provider: "capabilities2_runner_prompt/PromptLaunchRunner" \ No newline at end of file + "std_capabilities2/PromptLaunchRunner": + provider: "std_capabilities2/PromptLaunchRunner" \ No newline at end of file diff --git a/std_capabilities2/providers/prompt/PromptPlanRequestRunner.yaml b/std_capabilities2/providers/prompt/PromptPlanRequestRunner.yaml index bab68c9..d83cffb 100644 --- a/std_capabilities2/providers/prompt/PromptPlanRequestRunner.yaml +++ b/std_capabilities2/providers/prompt/PromptPlanRequestRunner.yaml @@ -6,8 +6,8 @@ spec_type: provider spec_version: 1.1 description: "The capability provider for the prompting for an execution plan. This can be used requesting a new plan at the beginning or when new data is available" -implements: capabilities2_runner_prompt/PromptPlanRequestRunner +implements: std_capabilities2/PromptPlanRequestRunner runner: capabilities2_runner::PromptPlanRequestRunner depends_on: - "capabilities2_runner_prompt/PromptLaunchRunner": - provider: "capabilities2_runner_prompt/PromptLaunchRunner" \ No newline at end of file + "std_capabilities2/PromptLaunchRunner": + provider: "std_capabilities2/PromptLaunchRunner" \ No newline at end of file diff --git a/std_capabilities2/providers/prompt/PromptPlanResponseRunner.yaml b/std_capabilities2/providers/prompt/PromptPlanResponseRunner.yaml index 76410e2..a7837b0 100644 --- a/std_capabilities2/providers/prompt/PromptPlanResponseRunner.yaml +++ b/std_capabilities2/providers/prompt/PromptPlanResponseRunner.yaml @@ -5,8 +5,8 @@ name: PromptPlanResponseRunner spec_type: provider spec_version: 1.1 description: "The capability provider for executing the received plan. This can be used to trigger the execution process of the new plan" -implements: capabilities2_runner_prompt/PromptPlanResponseRunner +implements: std_capabilities2/PromptPlanResponseRunner runner: capabilities2_runner::PromptPlanResponseRunner depends_on: - "capabilities2_runner_prompt/PromptLaunchRunner": - provider: "capabilities2_runner_prompt/PromptLaunchRunner" \ No newline at end of file + "std_capabilities2/PromptLaunchRunner": + provider: "std_capabilities2/PromptLaunchRunner" \ No newline at end of file diff --git a/std_capabilities2/providers/prompt/PromptPoseRunner.yaml b/std_capabilities2/providers/prompt/PromptPoseRunner.yaml index 55585e9..5cdfccd 100644 --- a/std_capabilities2/providers/prompt/PromptPoseRunner.yaml +++ b/std_capabilities2/providers/prompt/PromptPoseRunner.yaml @@ -5,8 +5,8 @@ name: PromptPoseRunner spec_type: provider spec_version: 1.1 description: "The capability provider for the prompting interface from prompt tools. This can be used for prompting pose of the robot." -implements: capabilities2_runner_prompt/PromptPoseRunner +implements: std_capabilities2/PromptPoseRunner runner: capabilities2_runner::PromptPoseRunner depends_on: - "capabilities2_runner_prompt/PromptLaunchRunner": - provider: "capabilities2_runner_prompt/PromptLaunchRunner" \ No newline at end of file + "std_capabilities2/PromptLaunchRunner": + provider: "std_capabilities2/PromptLaunchRunner" \ No newline at end of file diff --git a/std_capabilities2/providers/prompt/PromptTextRunner.yaml b/std_capabilities2/providers/prompt/PromptTextRunner.yaml index 8741bb4..0cf0385 100644 --- a/std_capabilities2/providers/prompt/PromptTextRunner.yaml +++ b/std_capabilities2/providers/prompt/PromptTextRunner.yaml @@ -5,8 +5,8 @@ name: PromptTextRunner spec_type: provider spec_version: 1.1 description: "The capability provider for the prompting interface from prompt tools. This can be used for prompting text." -implements: capabilities2_runner_prompt/PromptTextRunner +implements: std_capabilities2/PromptTextRunner runner: capabilities2_runner::PromptTextRunner depends_on: - "capabilities2_runner_prompt/PromptLaunchRunner": - provider: "capabilities2_runner_prompt/PromptLaunchRunner" \ No newline at end of file + "std_capabilities2/PromptLaunchRunner": + provider: "std_capabilities2/PromptLaunchRunner" \ No newline at end of file From d12f37d7b55fba26bd1731a398a4e1afc0f1c5dc Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Wed, 13 Nov 2024 19:18:49 +1100 Subject: [PATCH 028/133] bug fixing complete on capabilities2_fabric --- .../capabilities_fabric.hpp | 2 +- .../capabilities_xml_parser.hpp | 38 +++++++++---------- 2 files changed, 18 insertions(+), 22 deletions(-) diff --git a/capabilities2_fabric/include/capabilities2_executor/capabilities_fabric.hpp b/capabilities2_fabric/include/capabilities2_executor/capabilities_fabric.hpp index e000995..852eaaa 100644 --- a/capabilities2_fabric/include/capabilities2_executor/capabilities_fabric.hpp +++ b/capabilities2_fabric/include/capabilities2_executor/capabilities_fabric.hpp @@ -463,7 +463,7 @@ class CapabilitiesFabric : public rclcpp::Node return false; } - feedback->progress = "Plan tag checking successful"; + feedback->progress = "'Plan' tag checking successful"; goal_handle->publish_feedback(feedback); RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); diff --git a/capabilities2_fabric/include/capabilities2_executor/capabilities_xml_parser.hpp b/capabilities2_fabric/include/capabilities2_executor/capabilities_xml_parser.hpp index a9287be..cd43df5 100644 --- a/capabilities2_fabric/include/capabilities2_executor/capabilities_xml_parser.hpp +++ b/capabilities2_fabric/include/capabilities2_executor/capabilities_xml_parser.hpp @@ -125,21 +125,6 @@ bool check_tags(rclcpp::Logger logger, tinyxml2::XMLElement* element, std::vecto std::string nametag; std::string providertag; - // // Check and retrieve "name" and "provider" attributes safely - // if (element->QueryStringAttribute("name", &name) != tinyxml2::XML_SUCCESS || name == nullptr) - // { - // RCLCPP_ERROR(logger, "Missing 'name' attribute in XML element : %s", parameter_string.c_str()); - // rejected.push_back(parameter_string); - // return false; - // } - - // if (element->QueryStringAttribute("provider", &provider) != tinyxml2::XML_SUCCESS || provider == nullptr) - // { - // RCLCPP_ERROR(logger, "Missing 'provider' attribute in XML element : %s", parameter_string.c_str()); - // rejected.push_back(parameter_string); - // return false; - // } - std::string typetag(element->Name()); if (name) @@ -224,15 +209,26 @@ void extract_connections(tinyxml2::XMLElement* element, std::mapQueryStringAttribute("name", name); - element->QueryStringAttribute("provider", provider); + element->QueryStringAttribute("name", &name); + element->QueryStringAttribute("provider", &provider); std::string typetag(element->Name()); - std::string nametag(*name); - std::string providertag(*provider); + + std::string nametag; + std::string providertag; + + if (name) + nametag = name; + else + nametag = ""; + + if (provider) + providertag = provider; + else + providertag = ""; bool hasChildren = !element->NoChildren(); bool hasSiblings = !capabilities2_xml_parser::isLastElement(element); From 43f2b42da5b42af9d49fe51f147db679a011d659 Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Thu, 14 Nov 2024 02:31:23 +1100 Subject: [PATCH 029/133] updated lambda functions of runners in capabilities2_runner package. Runtime error at dependent launch file starting using capabilities2_launch_proxy --- .../config/{executor.yaml => fabric.yaml} | 2 +- .../docs/waypoint_runner_ex1.md | 57 ++++++++++++++ capabilities2_fabric/launch/fabric.launch.py | 2 +- capabilities2_fabric/plans/navigation_1.xml | 2 +- capabilities2_fabric/readme.md | 75 ++++++++++++++++++- .../capabilities2_runner/action_runner.hpp | 69 ++++++++--------- .../capabilities2_runner/service_runner.hpp | 74 ++++++++++-------- .../capabilities2_runner/topic_runner.hpp | 12 +-- capabilities2_runner_audio/CMakeLists.txt | 2 +- capabilities2_runner_audio/plugins.xml | 6 +- capabilities2_runner_bt/CMakeLists.txt | 2 +- capabilities2_runner_bt/plugins.xml | 2 +- capabilities2_runner_nav2/CMakeLists.txt | 2 +- capabilities2_runner_nav2/plugins.xml | 8 +- capabilities2_runner_prompt/CMakeLists.txt | 2 +- capabilities2_runner_prompt/plugins.xml | 10 +-- .../capabilities2_server/runner_cache.hpp | 31 ++++++-- docs/nav2_setup.md | 23 ++++++ readme.md | 3 +- 19 files changed, 283 insertions(+), 101 deletions(-) rename capabilities2_fabric/config/{executor.yaml => fabric.yaml} (74%) create mode 100644 capabilities2_fabric/docs/waypoint_runner_ex1.md create mode 100644 docs/nav2_setup.md diff --git a/capabilities2_fabric/config/executor.yaml b/capabilities2_fabric/config/fabric.yaml similarity index 74% rename from capabilities2_fabric/config/executor.yaml rename to capabilities2_fabric/config/fabric.yaml index 8dbc6e8..ce64389 100644 --- a/capabilities2_fabric/config/executor.yaml +++ b/capabilities2_fabric/config/fabric.yaml @@ -1,4 +1,4 @@ /**: ros__parameters: - plan_file_path: "install/capabilities2_fabric/share/capabilities2_fabric/plans/navigation_1.xml" + plan_file_path: "install/capabilities2_fabric/share/capabilities2_fabric/plans/navigation_1.xml" # WaypointRunner Example 1 # plan_file_path: "install/capabilities2_fabric/share/capabilities2_fabric/plans/default.xml" diff --git a/capabilities2_fabric/docs/waypoint_runner_ex1.md b/capabilities2_fabric/docs/waypoint_runner_ex1.md new file mode 100644 index 0000000..f2cf7da --- /dev/null +++ b/capabilities2_fabric/docs/waypoint_runner_ex1.md @@ -0,0 +1,57 @@ +## WaypointRunner Example 1 + +### Dependencies + +This example uses nav2 stack and turtlebot3. + +Run the following commands to install nav2 stack + +```bash +sudo apt install ros-humble-navigation2 ros-humble-nav2-bringup +``` + +Run the following commands to install turtlebot3 + +```bash +sudo apt install ros-humble-turtlebot3* +``` + +Clone the nav_stack default configuration and launch files to the same workspace if its not already availabe in the workspace. Capabilities2 Nav2 Runners are dependent on this package. + +```bash +cd src +git clone https://github.com/CollaborativeRoboticsLab/nav_stack.git +``` + +### Plan selection + +Uncomment the line related to `navigation_1.xml` in the `config/fabric,yaml` file + +### Build the package to apply changes + +In the workspace root run, + +```bash +colcon build +``` + +### Start the turtlebot simulation + +```bash +export TURTLEBOT3_MODEL=waffle +ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py +``` + +### Start the Capabilities2 Server + +```bash +source install/setup.bash +ros2 launch capabilities2_server server.launch.py +``` + +### Start the fabric + +```bash +source install/setup.bash +ros2 launch capabilities2_fabric fabric.launch.py +``` \ No newline at end of file diff --git a/capabilities2_fabric/launch/fabric.launch.py b/capabilities2_fabric/launch/fabric.launch.py index 243598c..2320523 100644 --- a/capabilities2_fabric/launch/fabric.launch.py +++ b/capabilities2_fabric/launch/fabric.launch.py @@ -15,7 +15,7 @@ def generate_launch_description(): LaunchDescription: The launch description for capabilities2 executor """ # load config file - executor_config = os.path.join(get_package_share_directory('capabilities2_fabric'), 'config', 'executor.yaml') + executor_config = os.path.join(get_package_share_directory('capabilities2_fabric'), 'config', 'fabric.yaml') executor = Node( package='capabilities2_fabric', diff --git a/capabilities2_fabric/plans/navigation_1.xml b/capabilities2_fabric/plans/navigation_1.xml index ac16237..1e10f14 100644 --- a/capabilities2_fabric/plans/navigation_1.xml +++ b/capabilities2_fabric/plans/navigation_1.xml @@ -1,6 +1,6 @@ - + \ No newline at end of file diff --git a/capabilities2_fabric/readme.md b/capabilities2_fabric/readme.md index b024286..96e449e 100644 --- a/capabilities2_fabric/readme.md +++ b/capabilities2_fabric/readme.md @@ -1,6 +1,75 @@ -# Capabilities2 Fabric +# Capabilities2_fabric -This is a executor that extracts the event relationship from the provided xml plan and transfers that to the capabilities2 server +Capabilities2_fabric is a ROS2 package that provides a system to coordinate and manage various capabilities as defined by the Capabilities2 framework. This package extends the functionality of the Capabilities2 package to implement a control framework based on capabilities. It is designed to parse an execution plan given via an XML file and then to identify connections between various capabilities in the system. -## Usage +Currently the system support 3 types of Control fuctions `sequential`, `parallel` and `recovery`, and multitude of Event functions. +## Features + +- Dynamic Capability Loading: Interacts with and manages capabilities defined by the capabilities2 framework. +- Flexible Workflow Execution: Parses XML-based plans and identifies event-driven callbacks for success, failure, or in-progress states. + + +## Launching capabilities2_fabric + +`capabilities2_fabric/plans` folder includes sample XML plans that can be used to test the system. New plans can be added to the same folder or a different location. + +Then modify the `capabilities2_fabric/config/fabric.yaml` file to change the active execution plan. +A number of plans are availabe with the package and included in the `fabric.yaml` file that has been commented out. Uncomment them to use. Make sure to leave only one line uncommented. + +```yaml +/**: + ros__parameters: + plan_file_path: "install/capabilities2_fabric/share/capabilities2_fabric/plans/default.xml" + +``` +Finally start the capabilities2 server. Run the following on a new terminal + +```bash +source install/setup.bash +ros2 launch capabilities2_fabric fabric.launch.py +``` + + +## XML Plan Parsing + +The capabilities2_fabric package relies on XML-based plans to define workflows. These plans specify the sequence of capabilities to execute, along with the associated parameters. The XML format includes tags for capabilities as events, and control flows enabling complex workflows to be structured in a modular way. + +Below is an example XML plan for configuring a set of capabilities: + +```xml + + + + + + + + + + + + + + + + + +``` + +## API + +| Node | Description | +| :--- | :--- | +| `capabilities2_Fabric` | Implemented the XML parsing and connection identification as well as communicating with `capabilities_server` to configure capability events | +| `capabilities2_File_Parser` | Reads an exection plan from a given path and sends it to the `capabilities2_fabric` node. Can be used as a sample action client to work with the `capabilities2_fabric` | + +| Action | Action Message | Description | +| :--- | :--- | :--- | +| `~/capabilities_fabric` | `Plan.action` | Receive and XML plan via the message for execution| + +## Samples and Testing + +### Navigation + +1. [WaypointRunner Example 1](./docs/waypoint_runner_ex1.md) \ No newline at end of file diff --git a/capabilities2_runner/include/capabilities2_runner/action_runner.hpp b/capabilities2_runner/include/capabilities2_runner/action_runner.hpp index 04d5d61..8b3052b 100644 --- a/capabilities2_runner/include/capabilities2_runner/action_runner.hpp +++ b/capabilities2_runner/include/capabilities2_runner/action_runner.hpp @@ -49,7 +49,7 @@ class ActionRunner : public RunnerBase // 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))) + if (!action_client_->wait_for_action_server(std::chrono::seconds(1000))) { RCLCPP_ERROR(node_->get_logger(), "%s failed to connect to action: %s", run_config_.interface.c_str(), action_name.c_str()); @@ -86,15 +86,16 @@ class ActionRunner : public RunnerBase if (response->return_code != action_msgs::srv::CancelGoal_Response::ERROR_NONE) { // throw runner_exception("failed to cancel runner"); + RCLCPP_WARN(node_->get_logger(), "Runner cancelation failed."); } - // publish event - if (events[execute_id].on_stopped) - { - events[execute_id].on_stopped(update_on_stopped(events[execute_id].on_stopped_param)); - execute_id += 1; - } - }); + // Trigger on_stopped event if defined + if (events[execute_id].on_stopped) + { + events[execute_id].on_stopped(update_on_stopped(events[execute_id].on_stopped_param)); + execute_id += 1; + } + }); // wait for action to be stopped. hold the thread for 2 seconds to help keep callbacks in scope // BUG: the line below does not work in jazzy build, so a workaround is used @@ -102,8 +103,12 @@ class ActionRunner : public RunnerBase auto timeout = std::chrono::steady_clock::now() + std::chrono::seconds(2); while (std::chrono::steady_clock::now() < timeout) { + // Check if the cancel operation is complete if (cancel_future.wait_for(std::chrono::milliseconds(100)) == std::future_status::ready) break; + + // Spin some work on the node to keep the callback within scope + rclcpp::spin_some(node_); } } catch (const rclcpp_action::exceptions::UnknownGoalHandleError& e) @@ -136,14 +141,19 @@ 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) + { + // Trigger on_started event if defined if (events[execute_id].on_started) { events[execute_id].on_started(update_on_started(events[execute_id].on_started_param)); execute_id += 1; } - + } + else + { + RCLCPP_ERROR(node_->get_logger(), "Goal was rejected by server"); + } // store goal handle to be used with stop funtion goal_handle_ = goal_handle; }; @@ -153,7 +163,7 @@ class ActionRunner : public RunnerBase [this](const typename rclcpp_action::ClientGoalHandle::WrappedResult& wrapped_result) { if (wrapped_result.code == rclcpp_action::ResultCode::SUCCEEDED) { - // send success event + // Trigger on_success event if defined if (events[execute_id].on_success) { result_ = wrapped_result.result; @@ -163,7 +173,7 @@ class ActionRunner : public RunnerBase } else { - // send terminated event + // Trigger on_failure event if defined if (events[execute_id].on_failure) { result_ = wrapped_result.result; @@ -176,42 +186,29 @@ class ActionRunner : public RunnerBase // trigger the action client with goal auto goal_handle_future = action_client_->async_send_goal(goal_msg, send_goal_options_); - // spin until send future completes - if (rclcpp::spin_until_future_complete(node_, goal_handle_future) != rclcpp::FutureReturnCode::SUCCESS) - { - RCLCPP_ERROR(node_->get_logger(), "send goal call failed"); - return std::nullopt; - } - - // get result future - typename rclcpp_action::ClientGoalHandle::SharedPtr goal_handle = goal_handle_future.get(); - if (!goal_handle) - { - RCLCPP_ERROR(node_->get_logger(), "Goal was rejected by server"); - return std::nullopt; - } - - auto result_future = action_client_->async_get_result(goal_handle); - // create a function to call for the result. the future will be returned to the caller and the caller // can provide a conversion function to handle the result - std::function result_callback = [this, result_future](tinyxml2::XMLElement* result) { - // wait for result - if (rclcpp::spin_until_future_complete(node_, result_future) != rclcpp::FutureReturnCode::SUCCESS) + std::function result_callback = [this, + goal_handle_future](tinyxml2::XMLElement* result) { + auto goal_handle = goal_handle_future.get(); + + if (!goal_handle) { - RCLCPP_ERROR(node_->get_logger(), "get result call failed"); + RCLCPP_ERROR(node_->get_logger(), "Failed to send goal"); return; } - // get wrapped result - typename rclcpp_action::ClientGoalHandle::WrappedResult wrapped_result = result_future.get(); + // Get the result asynchronously + auto result_future = action_client_->async_get_result(goal_handle); + + // Wait for result future + auto wrapped_result = result_future.get(); // convert the result if (wrapped_result.code == rclcpp_action::ResultCode::SUCCEEDED) { result = generate_result(wrapped_result.result); - return; } }; diff --git a/capabilities2_runner/include/capabilities2_runner/service_runner.hpp b/capabilities2_runner/include/capabilities2_runner/service_runner.hpp index 07e4cf1..10e205f 100644 --- a/capabilities2_runner/include/capabilities2_runner/service_runner.hpp +++ b/capabilities2_runner/include/capabilities2_runner/service_runner.hpp @@ -69,42 +69,56 @@ class ServiceRunner : public RunnerBase // generate a goal from parameters if provided auto request_msg = std::make_shared(generate_request(parameters)); - auto result_future = service_client_->async_send_request(request_msg); - + auto result_future = service_client_->async_send_request( + request_msg, [this](typename rclcpp::Client::SharedFuture future) { + if (!future.valid()) + { + RCLCPP_ERROR(node_->get_logger(), "get result call failed"); + + // Trigger failure event + if (events[execute_id].on_failure) + { + events[execute_id].on_failure(update_on_failure(events[execute_id].on_failure_param)); + execute_id += 1; + } + } + else + { + RCLCPP_INFO(node_->get_logger(), "get result call succeeded"); + + // Trigger success event + if (events[execute_id].on_success) + { + events[execute_id].on_success(update_on_success(events[execute_id].on_success_param)); + execute_id += 1; + } + } + }); + + // Trigger started event if defined if (events[execute_id].on_started) { events[execute_id].on_started(update_on_started(events[execute_id].on_started_param)); execute_id += 1; } - if (rclcpp::spin_until_future_complete(node_, result_future) == rclcpp::FutureReturnCode::SUCCESS) - { - // send success event - if (events[execute_id].on_success) - { - events[execute_id].on_success(update_on_success(events[execute_id].on_success_param)); - execute_id += 1; - } - } - else - { - RCLCPP_ERROR(node_->get_logger(), "get result call failed"); - - // send terminated event - if (events[execute_id].on_failure) - { - events[execute_id].on_failure(update_on_failure(events[execute_id].on_failure_param)); - execute_id += 1; - } - } - - // create a function to call for the result. the future will be returned to the caller and the caller - // can provide a conversion function to handle the result - - std::function result_callback = [this, &result_future](tinyxml2::XMLElement* result) mutable { - auto response = result_future.get(); - result = generate_response(response); - }; + // Define a callback function to handle the result once it's ready + std::function result_callback = + [this, &result_future](tinyxml2::XMLElement* result) mutable { + auto response = result_future.get(); + result = generate_response(response); + + // Ensure the future is ready before accessing the result + if (result_future.wait_for(std::chrono::seconds(0)) == std::future_status::ready) + { + auto response = result_future.get(); + result = generate_response(response); + } + else + { + RCLCPP_WARN(node_->get_logger(), "Result is not ready yet."); + } + }; return result_callback; } diff --git a/capabilities2_runner/include/capabilities2_runner/topic_runner.hpp b/capabilities2_runner/include/capabilities2_runner/topic_runner.hpp index aa1a147..5b45b3e 100644 --- a/capabilities2_runner/include/capabilities2_runner/topic_runner.hpp +++ b/capabilities2_runner/include/capabilities2_runner/topic_runner.hpp @@ -39,9 +39,8 @@ class TopicRunner : public RunnerBase init_base(node, run_config); // create an service client - subscription_ = - node_->create_subscription(topic_name, 1, - [this](const typename TopicT::SharedPtr msg) { this->callback(msg); }); + subscription_ = node_->create_subscription( + topic_name, 1, [this](const typename TopicT::SharedPtr msg) { this->callback(msg); }); } /** @@ -63,7 +62,7 @@ class TopicRunner : public RunnerBase execute_id += 1; } - if (!latest_message_) + if (latest_message_) { // send success event if (events[execute_id].on_success) @@ -88,7 +87,8 @@ class TopicRunner : public RunnerBase // can provide a conversion function to handle the result std::function result_callback = [this](tinyxml2::XMLElement* result) { - result = generate_message(latest_message_); + if (latest_message_) + result = generate_message(latest_message_); }; return result_callback; @@ -149,6 +149,6 @@ class TopicRunner : public RunnerBase typename rclcpp::Subscription::SharedPtr subscription_; - mutable typename TopicT::SharedPtr latest_message_ = nullptr; + mutable typename TopicT::SharedPtr latest_message_{ nullptr }; }; } // namespace capabilities2_runner \ No newline at end of file diff --git a/capabilities2_runner_audio/CMakeLists.txt b/capabilities2_runner_audio/CMakeLists.txt index f6cc0ce..4a1013e 100644 --- a/capabilities2_runner_audio/CMakeLists.txt +++ b/capabilities2_runner_audio/CMakeLists.txt @@ -48,7 +48,7 @@ ament_target_dependencies(${PROJECT_NAME} TINYXML2 ) -pluginlib_export_plugin_description_file(${PROJECT_NAME} plugins.xml) +pluginlib_export_plugin_description_file(capabilities2_runner plugins.xml) install(TARGETS ${PROJECT_NAME} EXPORT ${PROJECT_NAME} diff --git a/capabilities2_runner_audio/plugins.xml b/capabilities2_runner_audio/plugins.xml index 5f1e307..a1355d0 100644 --- a/capabilities2_runner_audio/plugins.xml +++ b/capabilities2_runner_audio/plugins.xml @@ -1,10 +1,10 @@ - - + + A plugin that provide speech to text capability - + A plugin that provide text to speech capability diff --git a/capabilities2_runner_bt/CMakeLists.txt b/capabilities2_runner_bt/CMakeLists.txt index 092a483..7cfd988 100644 --- a/capabilities2_runner_bt/CMakeLists.txt +++ b/capabilities2_runner_bt/CMakeLists.txt @@ -47,7 +47,7 @@ ament_target_dependencies(${PROJECT_NAME} TINYXML2 ) -pluginlib_export_plugin_description_file(${PROJECT_NAME} plugins.xml) +pluginlib_export_plugin_description_file(capabilities2_runner plugins.xml) install(TARGETS ${PROJECT_NAME} EXPORT ${PROJECT_NAME} diff --git a/capabilities2_runner_bt/plugins.xml b/capabilities2_runner_bt/plugins.xml index 1289f20..0763f3e 100644 --- a/capabilities2_runner_bt/plugins.xml +++ b/capabilities2_runner_bt/plugins.xml @@ -1,4 +1,4 @@ - + diff --git a/capabilities2_runner_nav2/CMakeLists.txt b/capabilities2_runner_nav2/CMakeLists.txt index df75cbc..ef912de 100644 --- a/capabilities2_runner_nav2/CMakeLists.txt +++ b/capabilities2_runner_nav2/CMakeLists.txt @@ -49,7 +49,7 @@ ament_target_dependencies(${PROJECT_NAME} TINYXML2 ) -pluginlib_export_plugin_description_file(${PROJECT_NAME} plugins.xml) +pluginlib_export_plugin_description_file(capabilities2_runner plugins.xml) install(TARGETS ${PROJECT_NAME} EXPORT ${PROJECT_NAME} diff --git a/capabilities2_runner_nav2/plugins.xml b/capabilities2_runner_nav2/plugins.xml index d72c114..b7fcdaf 100644 --- a/capabilities2_runner_nav2/plugins.xml +++ b/capabilities2_runner_nav2/plugins.xml @@ -1,15 +1,15 @@ - - + + A plugin that provide nav2 waypoint navigation capability - + A plugin that provide robot pose extraction capability - + A plugin that provide occupancy grid map extraction capability diff --git a/capabilities2_runner_prompt/CMakeLists.txt b/capabilities2_runner_prompt/CMakeLists.txt index 22295df..e82abd8 100644 --- a/capabilities2_runner_prompt/CMakeLists.txt +++ b/capabilities2_runner_prompt/CMakeLists.txt @@ -47,7 +47,7 @@ ament_target_dependencies(${PROJECT_NAME} TINYXML2 ) -pluginlib_export_plugin_description_file(${PROJECT_NAME} plugins.xml) +pluginlib_export_plugin_description_file(capabilities2_runner plugins.xml) install(TARGETS ${PROJECT_NAME} EXPORT ${PROJECT_NAME} diff --git a/capabilities2_runner_prompt/plugins.xml b/capabilities2_runner_prompt/plugins.xml index f842299..f7ea14e 100644 --- a/capabilities2_runner_prompt/plugins.xml +++ b/capabilities2_runner_prompt/plugins.xml @@ -1,20 +1,20 @@ - - + + A plugin that provide text prompting capability - + A plugin that provide pose prompting capability - + A plugin that provide occupancy grid map prompting capability - + A plugin that provide execution plan requesting capability diff --git a/capabilities2_server/include/capabilities2_server/runner_cache.hpp b/capabilities2_server/include/capabilities2_server/runner_cache.hpp index 39de3c0..c66defa 100644 --- a/capabilities2_server/include/capabilities2_server/runner_cache.hpp +++ b/capabilities2_server/include/capabilities2_server/runner_cache.hpp @@ -35,6 +35,17 @@ class RunnerCache // on_success = nullptr; } + /** + * @brief connect with ROS node logging interface + * + * @param node_logging_interface_ptr pointer to the ROS node logging interface + */ + void connect(rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_interface_ptr) + { + // set logger + node_logging_interface_ptr_ = node_logging_interface_ptr; + } + /** * @brief Add a runner to the cache * @@ -126,8 +137,10 @@ class RunnerCache if (on_started_capability != "") { - event_options.on_started = [this, &on_started_capability](tinyxml2::XMLElement* parameters) { + event_options.on_started = [this, &on_started_capability, &capability](tinyxml2::XMLElement* parameters) { runner_cache_[on_started_capability]->trigger(parameters); + RCLCPP_INFO(node_logging_interface_ptr_->get_logger(), "%s is triggering %s on_start", capability.c_str(), + on_started_capability.c_str()); }; tinyxml2::XMLDocument doc; @@ -142,8 +155,10 @@ class RunnerCache if (on_failure_capability != "") { - event_options.on_failure = [this, &on_failure_capability](tinyxml2::XMLElement* parameters) { + event_options.on_failure = [this, &on_failure_capability, &capability](tinyxml2::XMLElement* parameters) { runner_cache_[on_failure_capability]->trigger(parameters); + RCLCPP_INFO(node_logging_interface_ptr_->get_logger(), "%s is triggering %s on_failure", capability.c_str(), + on_failure_capability.c_str()); }; tinyxml2::XMLDocument doc; @@ -158,8 +173,10 @@ class RunnerCache if (on_success_capability != "") { - event_options.on_success = [this, &on_success_capability](tinyxml2::XMLElement* parameters) { + event_options.on_success = [this, &on_success_capability, &capability](tinyxml2::XMLElement* parameters) { runner_cache_[on_success_capability]->trigger(parameters); + RCLCPP_INFO(node_logging_interface_ptr_->get_logger(), "%s is triggering %s on_success", capability.c_str(), + on_success_capability.c_str()); }; tinyxml2::XMLDocument doc; @@ -174,8 +191,10 @@ class RunnerCache if (on_stopped_capability != "") { - event_options.on_stopped = [this, &on_stopped_capability](tinyxml2::XMLElement* parameters) { + event_options.on_stopped = [this, &on_stopped_capability, &capability](tinyxml2::XMLElement* parameters) { runner_cache_[on_stopped_capability]->trigger(parameters); + RCLCPP_INFO(node_logging_interface_ptr_->get_logger(), "%s is triggering %s on_stopped", capability.c_str(), + on_stopped_capability.c_str()); }; tinyxml2::XMLDocument doc; @@ -294,7 +313,6 @@ class RunnerCache return runner_cache_.find(capability) != runner_cache_.end(); } - private: // map capability to running model // capability / provider specs -> runner @@ -305,6 +323,9 @@ class RunnerCache // runner plugin loader pluginlib::ClassLoader runner_loader_; + + // logger + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_interface_ptr_; }; } // namespace capabilities2_server diff --git a/docs/nav2_setup.md b/docs/nav2_setup.md new file mode 100644 index 0000000..47a9b12 --- /dev/null +++ b/docs/nav2_setup.md @@ -0,0 +1,23 @@ +# Dependency installation for Nav2 Runners + +## Install nav2 stack + +```bash +sudo apt install ros-humble-navigation2 ros-humble-nav2-bringup +``` + +## Clone configuration + +Clone the nav_stack default configuration and launch files to the same workspace if its not already availabe in the workspace. Capabilities2 Nav2 Runners are dependent on this package. + +```bash +cd src +git clone https://github.com/CollaborativeRoboticsLab/nav_stack.git +``` + +## Turtlebot3 Simulation (Optional) +If using a simulated turtlebot3 for testing, install using following commands. + +```bash +sudo apt install ros-humble-turtlebot3* +``` \ No newline at end of file diff --git a/readme.md b/readme.md index 8af75b1..157ac94 100644 --- a/readme.md +++ b/readme.md @@ -32,8 +32,9 @@ Runners can be created using the runner API parent classes [here](./capabilities ## Setup Information - [Setup Instructions with devcontainer](./docs/setup_with_dev.md) - [Setup Instructions without devcontainer](./docs/setup.md) +- [Dependency installation for Nav2 Runners](./docs/nav2_setup.md) -## Startup information +## Quick Startup information ### Starting the Capabilities2 server From 301d29ea4c405d5240e5c0cc2e656f88af61b193 Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Fri, 15 Nov 2024 04:37:49 +1100 Subject: [PATCH 030/133] minor modifications --- capabilities2_fabric/plans/navigation_1.xml | 2 +- .../capabilities2_runner/action_runner.hpp | 2 + .../capabilities2_runner/launch_runner.hpp | 88 ++++++++++++------- .../launch_runner_old.hpp | 70 +++++++++++++++ .../waypoint_runner.hpp | 17 ++-- .../interfaces/nav2/WaypointRunner.yaml | 4 +- .../providers/nav2/Nav2LaunchRunner.yaml | 2 +- 7 files changed, 141 insertions(+), 44 deletions(-) create mode 100644 capabilities2_runner/include/capabilities2_runner/launch_runner_old.hpp diff --git a/capabilities2_fabric/plans/navigation_1.xml b/capabilities2_fabric/plans/navigation_1.xml index 1e10f14..b64663a 100644 --- a/capabilities2_fabric/plans/navigation_1.xml +++ b/capabilities2_fabric/plans/navigation_1.xml @@ -1,6 +1,6 @@ - + \ No newline at end of file diff --git a/capabilities2_runner/include/capabilities2_runner/action_runner.hpp b/capabilities2_runner/include/capabilities2_runner/action_runner.hpp index 8b3052b..e3d21ed 100644 --- a/capabilities2_runner/include/capabilities2_runner/action_runner.hpp +++ b/capabilities2_runner/include/capabilities2_runner/action_runner.hpp @@ -55,6 +55,8 @@ class ActionRunner : public RunnerBase action_name.c_str()); throw runner_exception("failed to connect to action server"); } + RCLCPP_INFO(node_->get_logger(), "%s connected with action: %s", run_config_.interface.c_str(), + action_name.c_str()); } /** diff --git a/capabilities2_runner/include/capabilities2_runner/launch_runner.hpp b/capabilities2_runner/include/capabilities2_runner/launch_runner.hpp index 91d98e4..593a773 100644 --- a/capabilities2_runner/include/capabilities2_runner/launch_runner.hpp +++ b/capabilities2_runner/include/capabilities2_runner/launch_runner.hpp @@ -2,9 +2,12 @@ #include #include -#include -#include -#include +#include +#include +#include +#include +#include +#include namespace capabilities2_runner { @@ -14,10 +17,13 @@ namespace capabilities2_runner * * Create a launch file runner to run a launch file based capability */ -class LaunchRunner : public NoTriggerActionRunner +class LaunchRunner : public RunnerBase { public: - LaunchRunner() : NoTriggerActionRunner() + /** + * @brief Constructor which needs to be empty due to plugin semantics + */ + LaunchRunner() : RunnerBase() { } @@ -29,42 +35,58 @@ class LaunchRunner : public NoTriggerActionRunnerget_logger(), "Failed to get package share directory: %s", e.what()); - throw runner_exception("failed to get package share directory"); - } + std::string command = "pkill -f 'ros2 launch " + get_package_name() + " " + run_config_.runner + "'"; - // resolve launch path - // get full path to launch file - // join package path with package name using path functions - std::string launch_file_path = std::filesystem::path(package_path).append(run_config_.runner).string(); + // Kill the launch process if still running + std::system(command.c_str()); + launch_thread_.join(); + } + } - // the launch file path - RCLCPP_DEBUG(node_->get_logger(), "launch file path: %s", launch_file_path.c_str()); + void startLaunchFile() + { + std::string command = "ros2 launch " + get_package_name() + " " + run_config_.runner; + RCLCPP_INFO(node_->get_logger(), "Executing command: %s", command.c_str()); - // create a launch goal - capabilities2_msgs::action::Launch::Goal goal; - goal.launch_file_path = launch_file_path; + // Run the command + int result = std::system(command.c_str()); - send_goal_options_.result_callback = nullptr; + if (result != 0) + { + RCLCPP_ERROR(node_->get_logger(), "Failed to launch file with command: %s", command.c_str()); + } + else + { + RCLCPP_INFO(node_->get_logger(), "Successfully launched file: %s", run_config_.runner.c_str()); + } + } - // launch runner using action client - action_client_->async_send_goal(goal, send_goal_options_); + // throw on trigger function + std::optional> trigger(tinyxml2::XMLElement* parameters) override + { + throw runner_exception("cannot trigger this is a no-trigger action runner"); } -private: - /** launch file path */ - std::string launch_file_path; + std::thread launch_thread_; }; -} // namespace capabilities2_runner +} // namespace capabilities2_runner \ No newline at end of file diff --git a/capabilities2_runner/include/capabilities2_runner/launch_runner_old.hpp b/capabilities2_runner/include/capabilities2_runner/launch_runner_old.hpp new file mode 100644 index 0000000..91d98e4 --- /dev/null +++ b/capabilities2_runner/include/capabilities2_runner/launch_runner_old.hpp @@ -0,0 +1,70 @@ +#pragma once + +#include +#include +#include +#include +#include + +namespace capabilities2_runner +{ + +/** + * @brief launch runner base class + * + * Create a launch file runner to run a launch file based capability + */ +class LaunchRunner : public NoTriggerActionRunner +{ +public: + LaunchRunner() : NoTriggerActionRunner() + { + } + + /** + * @brief Starter function for starting the launch runner + * + * @param node shared pointer to the capabilities node. Allows to use ros node related functionalities + * @param run_config runner configuration loaded from the yaml file + */ + virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config) override + { + // store node pointer and run_config + init_action(node, run_config, "capabilities_launch_proxy/launch"); + + // get the package path from environment variable + std::string package_path; + try + { + package_path = ament_index_cpp::get_package_share_directory(get_package_name()); + } + catch (const std::exception& e) + { + RCLCPP_ERROR(node_->get_logger(), "Failed to get package share directory: %s", e.what()); + throw runner_exception("failed to get package share directory"); + } + + // resolve launch path + // get full path to launch file + // join package path with package name using path functions + std::string launch_file_path = std::filesystem::path(package_path).append(run_config_.runner).string(); + + // the launch file path + RCLCPP_DEBUG(node_->get_logger(), "launch file path: %s", launch_file_path.c_str()); + + // create a launch goal + capabilities2_msgs::action::Launch::Goal goal; + goal.launch_file_path = launch_file_path; + + send_goal_options_.result_callback = nullptr; + + // launch runner using action client + action_client_->async_send_goal(goal, send_goal_options_); + } + +private: + /** launch file path */ + std::string launch_file_path; +}; + +} // namespace capabilities2_runner 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 2c00796..1afa4f2 100644 --- a/capabilities2_runner_nav2/include/capabilities2_runner_nav2/waypoint_runner.hpp +++ b/capabilities2_runner_nav2/include/capabilities2_runner_nav2/waypoint_runner.hpp @@ -7,7 +7,7 @@ #include #include -#include +#include #include @@ -20,7 +20,7 @@ namespace capabilities2_runner * Class to run waypointfollower action based capability * */ -class WayPointRunner : public ActionRunner +class WayPointRunner : public ActionRunner { public: WayPointRunner() : ActionRunner() @@ -35,7 +35,7 @@ class WayPointRunner : public ActionRunner */ virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config) override { - init_action(node, run_config, "follow_waypoints"); + init_action(node, run_config, "navigate_to_pose"); } protected: @@ -45,14 +45,16 @@ class WayPointRunner : public ActionRunner '' * @return ActionT::Goal the generated goal */ - virtual nav2_msgs::action::FollowWaypoints::Goal generate_goal(tinyxml2::XMLElement* parameters) override + virtual nav2_msgs::action::NavigateToPose::Goal generate_goal(tinyxml2::XMLElement* parameters) override { parameters_ = parameters; parameters_->QueryDoubleAttribute("x", &x); parameters_->QueryDoubleAttribute("y", &y); - nav2_msgs::action::FollowWaypoints::Goal goal_msg; + RCLCPP_INFO(node_->get_logger(), "%s is triggered with x: %d and y: %d", run_config_.interface.c_str(), x, y); + + nav2_msgs::action::NavigateToPose::Goal goal_msg; geometry_msgs::msg::PoseStamped pose_msg; global_frame_ = "map"; @@ -63,8 +65,9 @@ class WayPointRunner : public ActionRunner pose_msg.pose.position.x = x; pose_msg.pose.position.y = y; pose_msg.pose.position.z = 0.0; + pose_msg.pose.orientation.w = 1.0; // Set default orientation (facing forward) - goal_msg.poses.push_back(pose_msg); + goal_msg.pose = pose_msg; return goal_msg; } @@ -76,7 +79,7 @@ class WayPointRunner : public ActionRunner * @return nullptr */ virtual tinyxml2::XMLElement* - generate_result(const nav2_msgs::action::FollowWaypoints::Result::SharedPtr& result) override + generate_result(const nav2_msgs::action::NavigateToPose::Result::SharedPtr& result) override { return nullptr; } diff --git a/std_capabilities2/interfaces/nav2/WaypointRunner.yaml b/std_capabilities2/interfaces/nav2/WaypointRunner.yaml index 882ee56..0e7bec3 100644 --- a/std_capabilities2/interfaces/nav2/WaypointRunner.yaml +++ b/std_capabilities2/interfaces/nav2/WaypointRunner.yaml @@ -11,6 +11,6 @@ description: "This capability depends on the navigation stack functionalities an meters forward and 0.8 meters to the right side." interface: actions: - "/follow_waypoints": - type: "nav2_msgs::action::FollowWaypoints" + "/navigate_to_pose": + type: "nav2_msgs::action::NavigateToPose" description: "Waypoint follower interfacce of the Nav2 stack" \ No newline at end of file diff --git a/std_capabilities2/providers/nav2/Nav2LaunchRunner.yaml b/std_capabilities2/providers/nav2/Nav2LaunchRunner.yaml index 4a02375..4620f69 100644 --- a/std_capabilities2/providers/nav2/Nav2LaunchRunner.yaml +++ b/std_capabilities2/providers/nav2/Nav2LaunchRunner.yaml @@ -6,4 +6,4 @@ spec_type: provider spec_version: 1.1 description: "The capability provider for the nav2 stack" implements: std_capabilities2/Nav2LaunchRunner -runner: launch/nav2/nav2stack.launch.py +runner: nav2stack.launch.py From a50ea25b1c21bf8dc6afa49f10a505601c3ac616 Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Fri, 15 Nov 2024 04:39:24 +1100 Subject: [PATCH 031/133] rollback --- .../capabilities2_runner/launch_runner.hpp | 88 +++++++------------ .../launch_runner_old.hpp | 70 --------------- 2 files changed, 33 insertions(+), 125 deletions(-) delete mode 100644 capabilities2_runner/include/capabilities2_runner/launch_runner_old.hpp diff --git a/capabilities2_runner/include/capabilities2_runner/launch_runner.hpp b/capabilities2_runner/include/capabilities2_runner/launch_runner.hpp index 593a773..91d98e4 100644 --- a/capabilities2_runner/include/capabilities2_runner/launch_runner.hpp +++ b/capabilities2_runner/include/capabilities2_runner/launch_runner.hpp @@ -2,12 +2,9 @@ #include #include -#include -#include -#include -#include -#include -#include +#include +#include +#include namespace capabilities2_runner { @@ -17,13 +14,10 @@ namespace capabilities2_runner * * Create a launch file runner to run a launch file based capability */ -class LaunchRunner : public RunnerBase +class LaunchRunner : public NoTriggerActionRunner { public: - /** - * @brief Constructor which needs to be empty due to plugin semantics - */ - LaunchRunner() : RunnerBase() + LaunchRunner() : NoTriggerActionRunner() { } @@ -35,58 +29,42 @@ class LaunchRunner : public RunnerBase */ virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config) override { - // Start the launch file in a separate thread - launch_thread_ = std::thread(&LaunchRunner::startLaunchFile, this); - } + // store node pointer and run_config + init_action(node, run_config, "capabilities_launch_proxy/launch"); - /** - * @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"); - - // Join the thread and stop the launch if the node is shutting down - if (launch_thread_.joinable()) + // get the package path from environment variable + std::string package_path; + try { - std::string command = "pkill -f 'ros2 launch " + get_package_name() + " " + run_config_.runner + "'"; - - // Kill the launch process if still running - std::system(command.c_str()); - launch_thread_.join(); + package_path = ament_index_cpp::get_package_share_directory(get_package_name()); + } + catch (const std::exception& e) + { + RCLCPP_ERROR(node_->get_logger(), "Failed to get package share directory: %s", e.what()); + throw runner_exception("failed to get package share directory"); } - } - void startLaunchFile() - { - std::string command = "ros2 launch " + get_package_name() + " " + run_config_.runner; - RCLCPP_INFO(node_->get_logger(), "Executing command: %s", command.c_str()); + // resolve launch path + // get full path to launch file + // join package path with package name using path functions + std::string launch_file_path = std::filesystem::path(package_path).append(run_config_.runner).string(); - // Run the command - int result = std::system(command.c_str()); + // the launch file path + RCLCPP_DEBUG(node_->get_logger(), "launch file path: %s", launch_file_path.c_str()); - if (result != 0) - { - RCLCPP_ERROR(node_->get_logger(), "Failed to launch file with command: %s", command.c_str()); - } - else - { - RCLCPP_INFO(node_->get_logger(), "Successfully launched file: %s", run_config_.runner.c_str()); - } - } + // create a launch goal + capabilities2_msgs::action::Launch::Goal goal; + goal.launch_file_path = launch_file_path; - // throw on trigger function - std::optional> trigger(tinyxml2::XMLElement* parameters) override - { - throw runner_exception("cannot trigger this is a no-trigger action runner"); + send_goal_options_.result_callback = nullptr; + + // launch runner using action client + action_client_->async_send_goal(goal, send_goal_options_); } - std::thread launch_thread_; +private: + /** launch file path */ + std::string launch_file_path; }; -} // namespace capabilities2_runner \ No newline at end of file +} // namespace capabilities2_runner diff --git a/capabilities2_runner/include/capabilities2_runner/launch_runner_old.hpp b/capabilities2_runner/include/capabilities2_runner/launch_runner_old.hpp deleted file mode 100644 index 91d98e4..0000000 --- a/capabilities2_runner/include/capabilities2_runner/launch_runner_old.hpp +++ /dev/null @@ -1,70 +0,0 @@ -#pragma once - -#include -#include -#include -#include -#include - -namespace capabilities2_runner -{ - -/** - * @brief launch runner base class - * - * Create a launch file runner to run a launch file based capability - */ -class LaunchRunner : public NoTriggerActionRunner -{ -public: - LaunchRunner() : NoTriggerActionRunner() - { - } - - /** - * @brief Starter function for starting the launch runner - * - * @param node shared pointer to the capabilities node. Allows to use ros node related functionalities - * @param run_config runner configuration loaded from the yaml file - */ - virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config) override - { - // store node pointer and run_config - init_action(node, run_config, "capabilities_launch_proxy/launch"); - - // get the package path from environment variable - std::string package_path; - try - { - package_path = ament_index_cpp::get_package_share_directory(get_package_name()); - } - catch (const std::exception& e) - { - RCLCPP_ERROR(node_->get_logger(), "Failed to get package share directory: %s", e.what()); - throw runner_exception("failed to get package share directory"); - } - - // resolve launch path - // get full path to launch file - // join package path with package name using path functions - std::string launch_file_path = std::filesystem::path(package_path).append(run_config_.runner).string(); - - // the launch file path - RCLCPP_DEBUG(node_->get_logger(), "launch file path: %s", launch_file_path.c_str()); - - // create a launch goal - capabilities2_msgs::action::Launch::Goal goal; - goal.launch_file_path = launch_file_path; - - send_goal_options_.result_callback = nullptr; - - // launch runner using action client - action_client_->async_send_goal(goal, send_goal_options_); - } - -private: - /** launch file path */ - std::string launch_file_path; -}; - -} // namespace capabilities2_runner From 528dd1784f2386634c9cf820599b4be628e4c581 Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Fri, 15 Nov 2024 11:56:01 +1100 Subject: [PATCH 032/133] minor fix --- capabilities2_fabric/plans/navigation_1.xml | 3 ++- .../capabilities_launch_proxy.py | 1 + .../include/capabilities2_runner_nav2/waypoint_runner.hpp | 8 ++------ std_capabilities2/providers/nav2/Nav2LaunchRunner.yaml | 2 +- 4 files changed, 6 insertions(+), 8 deletions(-) diff --git a/capabilities2_fabric/plans/navigation_1.xml b/capabilities2_fabric/plans/navigation_1.xml index b64663a..d75a719 100644 --- a/capabilities2_fabric/plans/navigation_1.xml +++ b/capabilities2_fabric/plans/navigation_1.xml @@ -1,6 +1,7 @@ - + + \ No newline at end of file diff --git a/capabilities2_launch_proxy/capabilities2_launch_proxy/capabilities_launch_proxy.py b/capabilities2_launch_proxy/capabilities2_launch_proxy/capabilities_launch_proxy.py index 26a5845..644bf8c 100755 --- a/capabilities2_launch_proxy/capabilities2_launch_proxy/capabilities_launch_proxy.py +++ b/capabilities2_launch_proxy/capabilities2_launch_proxy/capabilities_launch_proxy.py @@ -260,6 +260,7 @@ def main(): # spin node in new thread executor = MultiThreadedExecutor() executor.add_node(capabilities_launch_proxy) + executor_thread = threading.Thread(target=executor.spin) executor_thread.start() 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 1afa4f2..337ffaa 100644 --- a/capabilities2_runner_nav2/include/capabilities2_runner_nav2/waypoint_runner.hpp +++ b/capabilities2_runner_nav2/include/capabilities2_runner_nav2/waypoint_runner.hpp @@ -52,16 +52,12 @@ class WayPointRunner : public ActionRunner parameters_->QueryDoubleAttribute("x", &x); parameters_->QueryDoubleAttribute("y", &y); - RCLCPP_INFO(node_->get_logger(), "%s is triggered with x: %d and y: %d", run_config_.interface.c_str(), x, y); + RCLCPP_INFO(node_->get_logger(), "%s is triggered with x: %f and y: %f", run_config_.interface.c_str(), x, y); nav2_msgs::action::NavigateToPose::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.header.frame_id = "map"; pose_msg.pose.position.x = x; pose_msg.pose.position.y = y; pose_msg.pose.position.z = 0.0; diff --git a/std_capabilities2/providers/nav2/Nav2LaunchRunner.yaml b/std_capabilities2/providers/nav2/Nav2LaunchRunner.yaml index 4620f69..4a02375 100644 --- a/std_capabilities2/providers/nav2/Nav2LaunchRunner.yaml +++ b/std_capabilities2/providers/nav2/Nav2LaunchRunner.yaml @@ -6,4 +6,4 @@ spec_type: provider spec_version: 1.1 description: "The capability provider for the nav2 stack" implements: std_capabilities2/Nav2LaunchRunner -runner: nav2stack.launch.py +runner: launch/nav2/nav2stack.launch.py From 713373e3165530cc43840abfe2514494690e989c Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Fri, 15 Nov 2024 15:50:01 +1100 Subject: [PATCH 033/133] worksssss without launch proxy --- .../capabilities_fabric.hpp | 20 +++++++++++++++---- capabilities2_fabric/plans/navigation_1.xml | 3 +-- .../providers/nav2/WaypointRunner.yaml | 5 +---- 3 files changed, 18 insertions(+), 10 deletions(-) diff --git a/capabilities2_fabric/include/capabilities2_executor/capabilities_fabric.hpp b/capabilities2_fabric/include/capabilities2_executor/capabilities_fabric.hpp index 852eaaa..26f443a 100644 --- a/capabilities2_fabric/include/capabilities2_executor/capabilities_fabric.hpp +++ b/capabilities2_fabric/include/capabilities2_executor/capabilities_fabric.hpp @@ -290,9 +290,7 @@ class CapabilitiesFabric : public rclcpp::Node goal_handle->publish_feedback(feedback); RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); - expected_providers_++; - interface_list.push_back(semantic_interface); - getProvider(semantic_interface, goal_handle, true); + interface_list_sem.push_back(semantic_interface); } } // if no semantic interfaces are availble for a given interface, add the interface instead @@ -302,10 +300,21 @@ class CapabilitiesFabric : public rclcpp::Node goal_handle->publish_feedback(feedback); RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); - expected_providers_++; interface_list.push_back(interface); + } + + expected_providers_ = interface_list.size() + interface_list_sem.size(); + + for (const auto& interface : interface_list) + { getProvider(interface, goal_handle, false); } + + for (const auto& sem_interface : interface_list_sem) + { + interface_list.push_back(sem_interface); + getProvider(interface, goal_handle, true); + } }); } @@ -836,6 +845,9 @@ class CapabilitiesFabric : public rclcpp::Node /** vector of connections */ std::map connection_map; + /** Interface List */ + std::vector interface_list_sem; + /** Interface List */ std::vector interface_list; diff --git a/capabilities2_fabric/plans/navigation_1.xml b/capabilities2_fabric/plans/navigation_1.xml index d75a719..52012c1 100644 --- a/capabilities2_fabric/plans/navigation_1.xml +++ b/capabilities2_fabric/plans/navigation_1.xml @@ -1,7 +1,6 @@ - - + \ No newline at end of file diff --git a/std_capabilities2/providers/nav2/WaypointRunner.yaml b/std_capabilities2/providers/nav2/WaypointRunner.yaml index 774229e..94497ba 100644 --- a/std_capabilities2/providers/nav2/WaypointRunner.yaml +++ b/std_capabilities2/providers/nav2/WaypointRunner.yaml @@ -6,7 +6,4 @@ spec_type: provider spec_version: 1.1 description: "The capability provider for the follow_waypoint interface" implements: std_capabilities2/WaypointRunner -runner: capabilities2_runner::WayPointRunner -depends_on: - "std_capabilities2/Nav2LaunchRunner": - provider: "std_capabilities2/Nav2LaunchRunner" \ No newline at end of file +runner: capabilities2_runner::WayPointRunner \ No newline at end of file From 02c3fa498eacda2e44269aa3fa62bbdd77722817 Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Mon, 18 Nov 2024 09:48:04 +1100 Subject: [PATCH 034/133] added standalone launch_runner inheriting from runner_base .have issues testing on humble due to bondcpp errror --- .../capabilities2_runner/launch_runner.hpp | 91 +++++++----- std_capabilities2/.clang-format | 64 --------- std_capabilities2/CMakeLists.txt | 21 --- .../audio/ListenerLaunchRunner.yaml | 12 -- .../interfaces/audio/ListenerRunner.yaml | 16 --- .../interfaces/audio/SpeakerLaunchRunner.yaml | 12 -- .../interfaces/audio/SpeakerRunner.yaml | 18 --- .../interfaces/nav2/Nav2LaunchRunner.yaml | 18 --- .../interfaces/nav2/OccupancyGridRunner.yaml | 17 --- .../interfaces/nav2/RobotPoseRunner.yaml | 17 --- .../interfaces/nav2/WaypointRunner.yaml | 16 --- .../interfaces/prompt/PromptLaunchRunner.yaml | 20 --- .../prompt/PromptOccupancyRunner.yaml | 17 --- .../prompt/PromptPlanRequestRunner.yaml | 38 ----- .../prompt/PromptPlanResponseRunner.yaml | 15 -- .../interfaces/prompt/PromptPoseRunner.yaml | 16 --- .../interfaces/prompt/PromptTextRunner.yaml | 14 -- .../launch/audio/listen.launch.py | 15 -- .../launch/audio/speak.launch.py | 15 -- .../launch/nav2/nav2stack.launch.py | 15 -- .../launch/prompt/prompttool.launch.py | 15 -- std_capabilities2/package.xml | 136 ------------------ .../providers/audio/ListenerLaunchRunner.yaml | 9 -- .../providers/audio/ListenerRunner.yaml | 12 -- .../providers/audio/SpeakerLaunchRunner.yaml | 9 -- .../providers/audio/SpeakerRunner.yaml | 12 -- .../providers/nav2/Nav2LaunchRunner.yaml | 9 -- .../providers/nav2/OccupancyGridRunner.yaml | 12 -- .../providers/nav2/RobotPoseRunner.yaml | 12 -- .../providers/nav2/WaypointRunner.yaml | 9 -- .../providers/prompt/PromptLaunchRunner.yaml | 9 -- .../prompt/PromptOccupancyRunner.yaml | 12 -- .../prompt/PromptPlanRequestRunner.yaml | 13 -- .../prompt/PromptPlanResponseRunner.yaml | 12 -- .../providers/prompt/PromptPoseRunner.yaml | 12 -- .../providers/prompt/PromptTextRunner.yaml | 12 -- 36 files changed, 56 insertions(+), 716 deletions(-) delete mode 100644 std_capabilities2/.clang-format delete mode 100644 std_capabilities2/CMakeLists.txt delete mode 100644 std_capabilities2/interfaces/audio/ListenerLaunchRunner.yaml delete mode 100644 std_capabilities2/interfaces/audio/ListenerRunner.yaml delete mode 100644 std_capabilities2/interfaces/audio/SpeakerLaunchRunner.yaml delete mode 100644 std_capabilities2/interfaces/audio/SpeakerRunner.yaml delete mode 100644 std_capabilities2/interfaces/nav2/Nav2LaunchRunner.yaml delete mode 100644 std_capabilities2/interfaces/nav2/OccupancyGridRunner.yaml delete mode 100644 std_capabilities2/interfaces/nav2/RobotPoseRunner.yaml delete mode 100644 std_capabilities2/interfaces/nav2/WaypointRunner.yaml delete mode 100644 std_capabilities2/interfaces/prompt/PromptLaunchRunner.yaml delete mode 100644 std_capabilities2/interfaces/prompt/PromptOccupancyRunner.yaml delete mode 100644 std_capabilities2/interfaces/prompt/PromptPlanRequestRunner.yaml delete mode 100644 std_capabilities2/interfaces/prompt/PromptPlanResponseRunner.yaml delete mode 100644 std_capabilities2/interfaces/prompt/PromptPoseRunner.yaml delete mode 100644 std_capabilities2/interfaces/prompt/PromptTextRunner.yaml delete mode 100644 std_capabilities2/launch/audio/listen.launch.py delete mode 100644 std_capabilities2/launch/audio/speak.launch.py delete mode 100644 std_capabilities2/launch/nav2/nav2stack.launch.py delete mode 100644 std_capabilities2/launch/prompt/prompttool.launch.py delete mode 100644 std_capabilities2/package.xml delete mode 100644 std_capabilities2/providers/audio/ListenerLaunchRunner.yaml delete mode 100644 std_capabilities2/providers/audio/ListenerRunner.yaml delete mode 100644 std_capabilities2/providers/audio/SpeakerLaunchRunner.yaml delete mode 100644 std_capabilities2/providers/audio/SpeakerRunner.yaml delete mode 100644 std_capabilities2/providers/nav2/Nav2LaunchRunner.yaml delete mode 100644 std_capabilities2/providers/nav2/OccupancyGridRunner.yaml delete mode 100644 std_capabilities2/providers/nav2/RobotPoseRunner.yaml delete mode 100644 std_capabilities2/providers/nav2/WaypointRunner.yaml delete mode 100644 std_capabilities2/providers/prompt/PromptLaunchRunner.yaml delete mode 100644 std_capabilities2/providers/prompt/PromptOccupancyRunner.yaml delete mode 100644 std_capabilities2/providers/prompt/PromptPlanRequestRunner.yaml delete mode 100644 std_capabilities2/providers/prompt/PromptPlanResponseRunner.yaml delete mode 100644 std_capabilities2/providers/prompt/PromptPoseRunner.yaml delete mode 100644 std_capabilities2/providers/prompt/PromptTextRunner.yaml diff --git a/capabilities2_runner/include/capabilities2_runner/launch_runner.hpp b/capabilities2_runner/include/capabilities2_runner/launch_runner.hpp index 91d98e4..3c41180 100644 --- a/capabilities2_runner/include/capabilities2_runner/launch_runner.hpp +++ b/capabilities2_runner/include/capabilities2_runner/launch_runner.hpp @@ -1,10 +1,14 @@ #pragma once #include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include +#include namespace capabilities2_runner { @@ -14,10 +18,13 @@ namespace capabilities2_runner * * Create a launch file runner to run a launch file based capability */ -class LaunchRunner : public NoTriggerActionRunner +class LaunchRunner : public RunnerBase { public: - LaunchRunner() : NoTriggerActionRunner() + /** + * @brief Constructor which needs to be empty due to plugin semantics + */ + LaunchRunner() : RunnerBase() { } @@ -29,42 +36,56 @@ class LaunchRunner : public NoTriggerActionRunnerget_logger(), "Failed to get package share directory: %s", e.what()); - throw runner_exception("failed to get package share directory"); - } + package_name = run_config_.runner.substr(0, run_config_.runner.find("/")); + launch_name = run_config_.runner.substr(run_config_.runner.find("/") + 1); + + std::string command = "source install/setup.bash && ros2 launch " + package_name + " " + launch_name; - // resolve launch path - // get full path to launch file - // join package path with package name using path functions - std::string launch_file_path = std::filesystem::path(package_path).append(run_config_.runner).string(); + int childPid = fork(); - // the launch file path - RCLCPP_DEBUG(node_->get_logger(), "launch file path: %s", launch_file_path.c_str()); + if (childPid == 0) + { + // Child process start + execlp("/bin/bash", "/bin/bash", "-c", command.c_str(), NULL); + perror("execlp"); // If execlp fails + exit(1); + // Child process end + } - // create a launch goal - capabilities2_msgs::action::Launch::Goal goal; - goal.launch_file_path = launch_file_path; + if (childPid == -1) + RCLCPP_ERROR(node_->get_logger(), "%s launch file from %s failed", launch_name.c_str(), package_name.c_str()); + else + RCLCPP_INFO(node_->get_logger(), "%s launch file from %s started with PID : %d", launch_name.c_str(), + package_name.c_str(), childPid); + } - send_goal_options_.result_callback = nullptr; + /** + * @brief stop function to cease functionality and shutdown + * + */ + virtual void stop() override + { + if (childPid != -1) + { + kill(childPid, SIGTERM); // Send termination signal to child + waitpid(childPid, NULL, 0); // Wait for child to terminate + RCLCPP_INFO(node_->get_logger(), "%s launch file from %s stopped : %d", launch_name.c_str(), + package_name.c_str()); + childPid = -1; + } + } - // launch runner using action client - action_client_->async_send_goal(goal, send_goal_options_); + // throw on trigger function + std::optional> trigger(tinyxml2::XMLElement* parameters) override + { + throw runner_exception("No Trigger as this is launch runner"); } -private: - /** launch file path */ - std::string launch_file_path; + pid_t childPid = -1; + std::string launch_name; + std::string package_name; }; -} // namespace capabilities2_runner +} // namespace capabilities2_runner \ No newline at end of file diff --git a/std_capabilities2/.clang-format b/std_capabilities2/.clang-format deleted file mode 100644 index 92effdd..0000000 --- a/std_capabilities2/.clang-format +++ /dev/null @@ -1,64 +0,0 @@ - -BasedOnStyle: Google -AccessModifierOffset: -2 -ConstructorInitializerIndentWidth: 2 -AlignEscapedNewlinesLeft: false -AlignTrailingComments: true -AllowAllParametersOfDeclarationOnNextLine: false -AllowShortIfStatementsOnASingleLine: false -AllowShortLoopsOnASingleLine: false -AllowShortFunctionsOnASingleLine: None -AlwaysBreakTemplateDeclarations: true -AlwaysBreakBeforeMultilineStrings: false -BreakBeforeBinaryOperators: false -BreakBeforeTernaryOperators: false -BreakConstructorInitializersBeforeComma: true -BinPackParameters: true -ColumnLimit: 150 -ConstructorInitializerAllOnOneLineOrOnePerLine: true -DerivePointerBinding: false -PointerBindsToType: true -ExperimentalAutoDetectBinPacking: false -IndentCaseLabels: true -MaxEmptyLinesToKeep: 1 -NamespaceIndentation: None -ObjCSpaceBeforeProtocolList: true -PenaltyBreakBeforeFirstCallParameter: 19 -PenaltyBreakComment: 60 -PenaltyBreakString: 1 -PenaltyBreakFirstLessLess: 1000 -PenaltyExcessCharacter: 1000 -PenaltyReturnTypeOnItsOwnLine: 90 -SpacesBeforeTrailingComments: 2 -Cpp11BracedListStyle: false -Standard: Auto -IndentWidth: 2 -TabWidth: 2 -UseTab: Never -IndentFunctionDeclarationAfterType: false -SpacesInParentheses: false -SpacesInAngles: false -SpaceInEmptyParentheses: false -SpacesInCStyleCastParentheses: false -SpaceAfterControlStatementKeyword: true -SpaceBeforeAssignmentOperators: true -ContinuationIndentWidth: 4 -SortIncludes: false -SpaceAfterCStyleCast: false - -# Configure each individual brace in BraceWrapping -BreakBeforeBraces: Custom - -# Control of individual brace wrapping cases -BraceWrapping: { - AfterClass: 'true' - AfterControlStatement: 'true' - AfterEnum : 'true' - AfterFunction : 'true' - AfterNamespace : 'true' - AfterStruct : 'true' - AfterUnion : 'true' - BeforeCatch : 'true' - BeforeElse : 'true' - IndentBraces : 'false' -} diff --git a/std_capabilities2/CMakeLists.txt b/std_capabilities2/CMakeLists.txt deleted file mode 100644 index 5287c56..0000000 --- a/std_capabilities2/CMakeLists.txt +++ /dev/null @@ -1,21 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(std_capabilities2) - -find_package(ament_cmake REQUIRED) - -# install interface files -install(DIRECTORY interfaces - DESTINATION share/${PROJECT_NAME} -) - -# install launch files -install(DIRECTORY launch - DESTINATION share/${PROJECT_NAME} -) - -# install providers files -install(DIRECTORY providers - DESTINATION share/${PROJECT_NAME} -) - -ament_package() diff --git a/std_capabilities2/interfaces/audio/ListenerLaunchRunner.yaml b/std_capabilities2/interfaces/audio/ListenerLaunchRunner.yaml deleted file mode 100644 index 8fac50f..0000000 --- a/std_capabilities2/interfaces/audio/ListenerLaunchRunner.yaml +++ /dev/null @@ -1,12 +0,0 @@ -%YAML 1.1 ---- -name: ListenerLaunchRunner -spec_version: 1.1 -spec_type: interface -description: "This capability implements the listener component of the audio stack and is a dependency for other capabilities - that use audio stack functionalities." -interface: - actions: - "/speech_to_text": - type: "hri_audio_msgs::action::SpeechToText" - description: "Speech to text action that listens to the surrounding sounds" diff --git a/std_capabilities2/interfaces/audio/ListenerRunner.yaml b/std_capabilities2/interfaces/audio/ListenerRunner.yaml deleted file mode 100644 index c8f35d7..0000000 --- a/std_capabilities2/interfaces/audio/ListenerRunner.yaml +++ /dev/null @@ -1,16 +0,0 @@ -%YAML 1.1 ---- -name: ListenerRunner -spec_version: 1.1 -spec_type: interface -description: "This capability depends on the listener capability of the audio stack and allow the robot to listen to sounds in the - environment and then interpret that in the text format. This enables the decision making authority such as an LLM to - understand the surrounding sounds, discussions regarding or with the robot. The capability can be triggered via - '' xml command.This generally needs to be followed by with a text - prompting tool capability such as '' that allows for transfer - of identified audio data as text to the decision making authority so that they can be used for future decision making." -interface: - actions: - "/speech_to_text": - type: "hri_audio_msgs::action::SpeechToText" - description: "Speech to text action that listens to the surrounding sounds" \ No newline at end of file diff --git a/std_capabilities2/interfaces/audio/SpeakerLaunchRunner.yaml b/std_capabilities2/interfaces/audio/SpeakerLaunchRunner.yaml deleted file mode 100644 index 31258b6..0000000 --- a/std_capabilities2/interfaces/audio/SpeakerLaunchRunner.yaml +++ /dev/null @@ -1,12 +0,0 @@ -%YAML 1.1 ---- -name: SpeakerLaunchRunner -spec_version: 1.1 -spec_type: interface -description: "This capability implements the speaker component of the audio stack and is a dependency for other capabilities - that use audio stack functionalities." -interface: - actions: - "/text_to_speech": - type: "hri_audio_msgs::action::TextToSpeech" - description: "Text to Speech action that allows speaking the text given" diff --git a/std_capabilities2/interfaces/audio/SpeakerRunner.yaml b/std_capabilities2/interfaces/audio/SpeakerRunner.yaml deleted file mode 100644 index e09b525..0000000 --- a/std_capabilities2/interfaces/audio/SpeakerRunner.yaml +++ /dev/null @@ -1,18 +0,0 @@ -%YAML 1.1 ---- -name: SpeakerRunner -spec_version: 1.1 -spec_type: interface -description: "This capability depends on the speaker capability of the audio stack and allow the robot to speak a given text. This - enables the decision making authority such as an LLM to communicate with voice. The capability can be triggered via - '' xml command. The '$value' represents what the robot - has to speak. As an example, if the robot recieves an command in the format of - '', the robot will speak 'how are you'. If an - response is expected for what the robot speaks, then it is required to have the listening capability triggered after - this capability is triggered successfully. This can be done via '' - xml command." -interface: - actions: - "/text_to_speech": - type: "hri_audio_msgs::action::TextToSpeech" - description: "Text to Speech action that allows speaking the text given" \ No newline at end of file diff --git a/std_capabilities2/interfaces/nav2/Nav2LaunchRunner.yaml b/std_capabilities2/interfaces/nav2/Nav2LaunchRunner.yaml deleted file mode 100644 index af8c598..0000000 --- a/std_capabilities2/interfaces/nav2/Nav2LaunchRunner.yaml +++ /dev/null @@ -1,18 +0,0 @@ -%YAML 1.1 ---- -name: Nav2LaunchRunner -spec_version: 1.1 -spec_type: interface -description: "This capability impliments the basic navigation stack and is a dependency for other capabilities that depend - on navigation stack functionalities." -interface: - topics: - "goal": - type: "geometry_msgs/PoseStamped" - description: "Two dimensional navigation goal to which the robot should try to navigate. This goal should have the x - and y components of the pose. position submessage and the z component of the pose.orientation submessage - filled out. The goal should be in the /map tf frame, and the header.frame_id should be set to '/map'." - "cmd_vel": - type: "geometry_msgs/Twist" - description: "Command velocities for a mobile base to follow. Whether or not the command velocities are holonomic is - determined by the provider of this capability and the capabilities on which it depends." diff --git a/std_capabilities2/interfaces/nav2/OccupancyGridRunner.yaml b/std_capabilities2/interfaces/nav2/OccupancyGridRunner.yaml deleted file mode 100644 index 4215d25..0000000 --- a/std_capabilities2/interfaces/nav2/OccupancyGridRunner.yaml +++ /dev/null @@ -1,17 +0,0 @@ -%YAML 1.1 ---- -name: OccupancyGridRunner -spec_version: 1.1 -spec_type: interface -description: "This capability depends on the navigation stack functionalities and allows the extraction of the occupancy grid map - produced by the robot's mapping or localization system. This extracted map can be used by the decision making authority - to understand the environment around the robot as well as possible navigation paths. This capability can be triggered - with '' xml command. Upon the successful execution of this - capability, another capability that allows occupancy grid data transfer to the decision making authority needs to be - triggered. Such a capability can be triggered via '' - xml command." -interface: - topics: - "/map": - type: "nav_msgs::msg::OccupancyGrid" - description: "Standard ros2 message interface containing occupancy grid map of the robot" \ No newline at end of file diff --git a/std_capabilities2/interfaces/nav2/RobotPoseRunner.yaml b/std_capabilities2/interfaces/nav2/RobotPoseRunner.yaml deleted file mode 100644 index 49a2871..0000000 --- a/std_capabilities2/interfaces/nav2/RobotPoseRunner.yaml +++ /dev/null @@ -1,17 +0,0 @@ -%YAML 1.1 ---- -name: RobotPoseRunner -spec_version: 1.1 -spec_type: interface -description: "This capability depends on the navigation stack functionalities and allows the extraction of the pose of the robot from - the odoemtry or localization system. This extracted robot pose can be used by the decision making authority to understand - the position and orientation of the robot. This capability can be triggered with an xml command as - ''. Upon the successful execution of this capability, another capability - that allows robot pose prompting needs to be triggered for transfer of robot pose data to the decision making authority for - future decision making. '' can be used as an robot pose prompting - capability for this." -interface: - topics: - "/pose": - type: "geometry_msgs::msg::PoseWithCovarianceStamped" - description: "Standard ros2 message interface containing position and orientation of the robot" \ No newline at end of file diff --git a/std_capabilities2/interfaces/nav2/WaypointRunner.yaml b/std_capabilities2/interfaces/nav2/WaypointRunner.yaml deleted file mode 100644 index 0e7bec3..0000000 --- a/std_capabilities2/interfaces/nav2/WaypointRunner.yaml +++ /dev/null @@ -1,16 +0,0 @@ -%YAML 1.1 ---- -name: WaypointRunner -spec_version: 1.1 -spec_type: interface -description: "This capability depends on the navigation stack functionalities and allow the robot to navigate to a location given - by two dimensional coordinates. This capability can be used by the decision making authority such as an LLM to move - the robot to a required position and orientation. This capability can be triggered via the xml command - ''. '$value' represents a value in meters. - As an example ' means the robot will move 1.2 - meters forward and 0.8 meters to the right side." -interface: - actions: - "/navigate_to_pose": - type: "nav2_msgs::action::NavigateToPose" - description: "Waypoint follower interfacce of the Nav2 stack" \ No newline at end of file diff --git a/std_capabilities2/interfaces/prompt/PromptLaunchRunner.yaml b/std_capabilities2/interfaces/prompt/PromptLaunchRunner.yaml deleted file mode 100644 index c8e43c3..0000000 --- a/std_capabilities2/interfaces/prompt/PromptLaunchRunner.yaml +++ /dev/null @@ -1,20 +0,0 @@ -%YAML 1.1 ---- -name: PromptLaunchRunner -spec_version: 1.1 -spec_type: interface -description: "This capability implements the prompt tools stack and is a dependency for other capabilities - that use prompting functionalities." -interface: - topics: - "/history": - type: "prompt_msgs/msg/PromptHistory" - description: "The history of prompts sent to the Prompt Bridge using the service interface" - services: - "/prompt": - type: "prompt_msgs/srv/Prompt" - description: "The prompt to send to the Prompt Bridge. returns the generated response" - actions: - "/prompt": - type: "prompt_msgs/action/Prompt" - description: "The prompt goal that requires a response, the action interface publishes partial results. returns the generated response" diff --git a/std_capabilities2/interfaces/prompt/PromptOccupancyRunner.yaml b/std_capabilities2/interfaces/prompt/PromptOccupancyRunner.yaml deleted file mode 100644 index abc09cf..0000000 --- a/std_capabilities2/interfaces/prompt/PromptOccupancyRunner.yaml +++ /dev/null @@ -1,17 +0,0 @@ -%YAML 1.1 ---- -name: PromptOccupancyRunner -spec_version: 1.1 -spec_type: interface -description: "This capability depends on the prompt tools stack functionalities and allows an decision making authority such as an - LLM to recieve occupancy grid data from the robot. This capability can be trigged with the xml command - ''. This will prompt the nav_msgs/occupancy_grid - msg in the xml format and would allow the decision making authority to understand the surrounding environment of the - robot. Every execution plan need to have this as one of the last and compulsory capabilities to be executed this at - the end, so that decision making authority is updated about the latest environment information which can then be used - for future planning." -interface: - services: - "/prompt": - type: "prompt_msgs/srv/Prompt" - description: "This service focuses on prompting occupancy grid maps to decision making authority." \ No newline at end of file diff --git a/std_capabilities2/interfaces/prompt/PromptPlanRequestRunner.yaml b/std_capabilities2/interfaces/prompt/PromptPlanRequestRunner.yaml deleted file mode 100644 index 5299b0d..0000000 --- a/std_capabilities2/interfaces/prompt/PromptPlanRequestRunner.yaml +++ /dev/null @@ -1,38 +0,0 @@ -%YAML 1.1 ---- -name: PromptPlanRequestRunner -spec_version: 1.1 -spec_type: interface -description: "This capability depends on the prompt tools stack functionalities and allows an decision making authority such as an - LLM to recieve a request to create a new plan at the end of an execution plan. This capability can can be trigged with - an xml command such as ''. This will - prompt the decision making authority to create a new execution plan either to continue the current plan or to focus on - a new task depending on the state of the robot. This capability needs to be in every execution plan created and needs - be the capability before the last in any plan. Any and all execution plans calculated by this capability need to be - contained within and xml tags. Capabilities within the execution plan can be organised using sqeuntial, - parallel and recovery control flows. Capabilities in the sequential control flow will be triggered one after the other - depending on precceding capability's successful execution. Capabilities under sequential control flow need to be contained - within xml tags. Capabilities in the parallel control flow will be triggered all - at once and will be executed parallely. Capabilities under parallel control flow needs to come within - xml tags. Capabilities in the recovery control flow will be triggered one after the other depending only on - precceding capability's failed execution. Capabilities under recovery control flow need to come within - xml tags. Upon the successful execution of this capability, another capability need to be executed for verifying - and starting the execution. It can be done via '' - xml command. A most basic plan with all compulsory runners is as follows, - - - - - - - - - - - - " -interface: - services: - "/prompt": - type: "prompt_msgs/srv/Prompt" - description: "This capability focuses on capturing speech towards robot as text and transfering them to decision making authority." \ No newline at end of file diff --git a/std_capabilities2/interfaces/prompt/PromptPlanResponseRunner.yaml b/std_capabilities2/interfaces/prompt/PromptPlanResponseRunner.yaml deleted file mode 100644 index 145879e..0000000 --- a/std_capabilities2/interfaces/prompt/PromptPlanResponseRunner.yaml +++ /dev/null @@ -1,15 +0,0 @@ -%YAML 1.1 ---- -name: PromptPlanResponseRunner -spec_version: 1.1 -spec_type: interface -description: "This capability depends on the prompt tools stack functionalities and allows an decision making authority such as an - LLM to verify new plan and start the execution at the end of the old execution plan. The capability can be trigged with - an command such as ''. This needs to be triggered - at the very end of each plan so that the plan created by the decision making authority such as an LLM can be transferred - to the capabilities2 executor for connection extraction and future triggering." -interface: - services: - "/prompt": - type: "prompt_msgs/srv/Prompt" - description: "This capability focuses on capturing speech towards robot as text and transfering them to decision making authority." \ No newline at end of file diff --git a/std_capabilities2/interfaces/prompt/PromptPoseRunner.yaml b/std_capabilities2/interfaces/prompt/PromptPoseRunner.yaml deleted file mode 100644 index 3b2c479..0000000 --- a/std_capabilities2/interfaces/prompt/PromptPoseRunner.yaml +++ /dev/null @@ -1,16 +0,0 @@ -%YAML 1.1 ---- -name: PromptPoseRunner -spec_version: 1.1 -spec_type: interface -description: "This capability depends on the prompt tools stack functionalities and allows an decision making authority such as an - LLM to recieve robot pose and orientation data from the robot. This capability can be trigged with the xml command - ''. This will prompt the geometry_msgs/Pose msg in the xml - format and would allow the decision making authority to understand the position of the robot. Every execution plan - need to have this as one of the last compulsory capabilities to be executed, so that decision making authority is - updated about the latest position and orientation information of the robot which can then be used for future planning." -interface: - services: - "/prompt": - type: "prompt_msgs/srv/Prompt" - description: "This service focuses on capturing pose (position and orientation) of the robot and transfering them to decision making authority." \ No newline at end of file diff --git a/std_capabilities2/interfaces/prompt/PromptTextRunner.yaml b/std_capabilities2/interfaces/prompt/PromptTextRunner.yaml deleted file mode 100644 index ca6e4ea..0000000 --- a/std_capabilities2/interfaces/prompt/PromptTextRunner.yaml +++ /dev/null @@ -1,14 +0,0 @@ -%YAML 1.1 ---- -name: PromptTextRunner -spec_version: 1.1 -spec_type: interface -description: "This capability depends on the prompt tools stack functionalities and allows an decision making authority such as an - LLM to recieve audio data as text from the robot. This capability can be trigged with the xml command - ''. This will prompt the decision making authority in the xml - format and would allow the decision making authority to understand the audio input the robot receives." -interface: - services: - "/prompt": - type: "prompt_msgs/srv/Prompt" - description: "This capability focuses on capturing speech towards robot as text and transfering them to decision making authority." \ No newline at end of file diff --git a/std_capabilities2/launch/audio/listen.launch.py b/std_capabilities2/launch/audio/listen.launch.py deleted file mode 100644 index 7d29c1f..0000000 --- a/std_capabilities2/launch/audio/listen.launch.py +++ /dev/null @@ -1,15 +0,0 @@ -from launch import LaunchDescription -from launch.actions import IncludeLaunchDescription -from launch.launch_description_sources import PythonLaunchDescriptionSource -from ament_index_python.packages import get_package_share_directory -import os - -def generate_launch_description(): - - listen_launch_path = os.path.join(get_package_share_directory('hri_audio_stack'), 'launch', 'listen.launch.py') - - return LaunchDescription([ - IncludeLaunchDescription( - PythonLaunchDescriptionSource(listen_launch_path), - ) - ]) \ No newline at end of file diff --git a/std_capabilities2/launch/audio/speak.launch.py b/std_capabilities2/launch/audio/speak.launch.py deleted file mode 100644 index 1663f2b..0000000 --- a/std_capabilities2/launch/audio/speak.launch.py +++ /dev/null @@ -1,15 +0,0 @@ -from launch import LaunchDescription -from launch.actions import IncludeLaunchDescription -from launch.launch_description_sources import PythonLaunchDescriptionSource -from ament_index_python.packages import get_package_share_directory -import os - -def generate_launch_description(): - - speak_launch_path = os.path.join(get_package_share_directory('hri_audio_stack'), 'launch', 'speak.launch.py') - - return LaunchDescription([ - IncludeLaunchDescription( - PythonLaunchDescriptionSource(speak_launch_path), - ) - ]) \ No newline at end of file diff --git a/std_capabilities2/launch/nav2/nav2stack.launch.py b/std_capabilities2/launch/nav2/nav2stack.launch.py deleted file mode 100644 index 7190147..0000000 --- a/std_capabilities2/launch/nav2/nav2stack.launch.py +++ /dev/null @@ -1,15 +0,0 @@ -from launch import LaunchDescription -from launch.actions import IncludeLaunchDescription -from launch.launch_description_sources import PythonLaunchDescriptionSource -from ament_index_python.packages import get_package_share_directory -import os - -def generate_launch_description(): - - navstack_launch_path = os.path.join(get_package_share_directory('nav_stack'), 'launch', 'system.launch.py') - - return LaunchDescription([ - IncludeLaunchDescription( - PythonLaunchDescriptionSource(navstack_launch_path), - ) - ]) \ No newline at end of file diff --git a/std_capabilities2/launch/prompt/prompttool.launch.py b/std_capabilities2/launch/prompt/prompttool.launch.py deleted file mode 100644 index 1594ae3..0000000 --- a/std_capabilities2/launch/prompt/prompttool.launch.py +++ /dev/null @@ -1,15 +0,0 @@ -from launch import LaunchDescription -from launch.actions import IncludeLaunchDescription -from launch.launch_description_sources import PythonLaunchDescriptionSource -from ament_index_python.packages import get_package_share_directory -import os - -def generate_launch_description(): - - prompt_launch_path = os.path.join(get_package_share_directory('prompt_bridge'), 'launch', 'prompt_bridge.launch.py') - - return LaunchDescription([ - IncludeLaunchDescription( - PythonLaunchDescriptionSource(prompt_launch_path), - ) - ]) \ No newline at end of file diff --git a/std_capabilities2/package.xml b/std_capabilities2/package.xml deleted file mode 100644 index 8bfe758..0000000 --- a/std_capabilities2/package.xml +++ /dev/null @@ -1,136 +0,0 @@ - - - - std_capabilities2 - 0.0.0 - TODO: Package description - kalana - BSD - - ament_cmake - - ament_lint_auto - ament_lint_common - - - ament_cmake - - - - interfaces/nav2/WaypointRunner.yaml - - - - interfaces/nav2/OccupancyGridRunner.yaml - - - - interfaces/nav2/RobotPoseRunner.yaml - - - - interfaces/nav2/Nav2LaunchRunner.yaml - - - - - interfaces/prompt/PromptLaunchRunner.yaml - - - - interfaces/prompt/PromptTextRunner.yaml - - - - interfaces/prompt/PromptPoseRunner.yaml - - - - interfaces/prompt/PromptOccupancyRunner.yaml - - - - interfaces/prompt/PromptPlanRequestRunner.yaml - - - - interfaces/prompt/PromptPlanResponseRunner.yaml - - - - - interfaces/audio/ListenerRunner.yaml - - - - interfaces/audio/SpeakerRunner.yaml - - - - interfaces/audio/ListenerLaunchRunner.yaml - - - - interfaces/audio/SpeakerLaunchRunner.yaml - - - - - providers/nav2/RobotPoseRunner.yaml - - - - providers/nav2/OccupancyGridRunner.yaml - - - - providers/nav2/WaypointRunner.yaml - - - - providers/nav2/Nav2LaunchRunner.yaml - - - - - providers/prompt/PromptLaunchRunner.yaml - - - - providers/prompt/PromptTextRunner.yaml - - - - providers/prompt/PromptPoseRunner.yaml - - - - providers/prompt/PromptOccupancyRunner.yaml - - - - providers/prompt/PromptPlanRequestRunner.yaml - - - - providers/prompt/PromptPlanResponseRunner.yaml - - - - - providers/audio/ListenerRunner.yaml - - - - providers/audio/SpeakerRunner.yaml - - - - providers/audio/ListenerLaunchRunner.yaml - - - - providers/audio/SpeakerLaunchRunner.yaml - - - diff --git a/std_capabilities2/providers/audio/ListenerLaunchRunner.yaml b/std_capabilities2/providers/audio/ListenerLaunchRunner.yaml deleted file mode 100644 index 8572453..0000000 --- a/std_capabilities2/providers/audio/ListenerLaunchRunner.yaml +++ /dev/null @@ -1,9 +0,0 @@ -# the empty provider -%YAML 1.1 ---- -name: ListenerLaunchRunner -spec_type: provider -spec_version: 1.1 -description: "The capability provider for the audio stack" -implements: std_capabilities2/ListenerLaunchRunner -runner: launch/audio/listen.launch.py diff --git a/std_capabilities2/providers/audio/ListenerRunner.yaml b/std_capabilities2/providers/audio/ListenerRunner.yaml deleted file mode 100644 index ec94941..0000000 --- a/std_capabilities2/providers/audio/ListenerRunner.yaml +++ /dev/null @@ -1,12 +0,0 @@ -# the empty provider -%YAML 1.1 ---- -name: ListenerRunner -spec_type: provider -spec_version: 1.1 -description: The capability provider for the speech_to_text interface -implements: std_capabilities2/ListenerRunner -runner: capabilities2_runner::ListenerRunner -depends_on: - "std_capabilities2/ListenerLaunchRunner": - provider: "std_capabilities2/ListenerLaunchRunner" \ No newline at end of file diff --git a/std_capabilities2/providers/audio/SpeakerLaunchRunner.yaml b/std_capabilities2/providers/audio/SpeakerLaunchRunner.yaml deleted file mode 100644 index 1f250db..0000000 --- a/std_capabilities2/providers/audio/SpeakerLaunchRunner.yaml +++ /dev/null @@ -1,9 +0,0 @@ -# the empty provider -%YAML 1.1 ---- -name: SpeakerLaunchRunner -spec_type: provider -spec_version: 1.1 -description: "The capability provider for the audio stack" -implements: std_capabilities2/SpeakerLaunchRunner -runner: launch/audio/speak.launch.py \ No newline at end of file diff --git a/std_capabilities2/providers/audio/SpeakerRunner.yaml b/std_capabilities2/providers/audio/SpeakerRunner.yaml deleted file mode 100644 index ecc5656..0000000 --- a/std_capabilities2/providers/audio/SpeakerRunner.yaml +++ /dev/null @@ -1,12 +0,0 @@ -# the empty provider -%YAML 1.1 ---- -name: SpeakerRunner -spec_type: provider -spec_version: 1.1 -description: The capability provider for the text_to_speech interface -implements: std_capabilities2/SpeakerRunner -runner: capabilities2_runner::SpeakerRunner -depends_on: - "std_capabilities2/SpeakerLaunchRunner": - provider: "std_capabilities2/SpeakerLaunchRunner" \ No newline at end of file diff --git a/std_capabilities2/providers/nav2/Nav2LaunchRunner.yaml b/std_capabilities2/providers/nav2/Nav2LaunchRunner.yaml deleted file mode 100644 index 4a02375..0000000 --- a/std_capabilities2/providers/nav2/Nav2LaunchRunner.yaml +++ /dev/null @@ -1,9 +0,0 @@ -# the base provider -%YAML 1.1 ---- -name: Nav2LaunchRunner -spec_type: provider -spec_version: 1.1 -description: "The capability provider for the nav2 stack" -implements: std_capabilities2/Nav2LaunchRunner -runner: launch/nav2/nav2stack.launch.py diff --git a/std_capabilities2/providers/nav2/OccupancyGridRunner.yaml b/std_capabilities2/providers/nav2/OccupancyGridRunner.yaml deleted file mode 100644 index f9e6dae..0000000 --- a/std_capabilities2/providers/nav2/OccupancyGridRunner.yaml +++ /dev/null @@ -1,12 +0,0 @@ -# the empty provider -%YAML 1.1 ---- -name: OccupancyGridRunner -spec_type: provider -spec_version: 1.1 -description: "The capability provider for the robot's occupancy grid map interface" -implements: std_capabilities2/OccupancyGridRunner -runner: capabilities2_runner::OccupancyGridRunner -depends_on: - "std_capabilities2/Nav2LaunchRunner": - provider: "std_capabilities2/Nav2LaunchRunner" \ No newline at end of file diff --git a/std_capabilities2/providers/nav2/RobotPoseRunner.yaml b/std_capabilities2/providers/nav2/RobotPoseRunner.yaml deleted file mode 100644 index 7f17dbc..0000000 --- a/std_capabilities2/providers/nav2/RobotPoseRunner.yaml +++ /dev/null @@ -1,12 +0,0 @@ -# the empty provider -%YAML 1.1 ---- -name: RobotPoseRunner -spec_type: provider -spec_version: 1.1 -description: "The capability provider for the robot's odometry interface" -implements: std_capabilities2/RobotPoseRunner -runner: capabilities2_runner::RobotPoseRunner -depends_on: - "std_capabilities2/Nav2LaunchRunner": - provider: "std_capabilities2/Nav2LaunchRunner" \ No newline at end of file diff --git a/std_capabilities2/providers/nav2/WaypointRunner.yaml b/std_capabilities2/providers/nav2/WaypointRunner.yaml deleted file mode 100644 index 94497ba..0000000 --- a/std_capabilities2/providers/nav2/WaypointRunner.yaml +++ /dev/null @@ -1,9 +0,0 @@ -# the empty provider -%YAML 1.1 ---- -name: WaypointRunner -spec_type: provider -spec_version: 1.1 -description: "The capability provider for the follow_waypoint interface" -implements: std_capabilities2/WaypointRunner -runner: capabilities2_runner::WayPointRunner \ No newline at end of file diff --git a/std_capabilities2/providers/prompt/PromptLaunchRunner.yaml b/std_capabilities2/providers/prompt/PromptLaunchRunner.yaml deleted file mode 100644 index 94b7dec..0000000 --- a/std_capabilities2/providers/prompt/PromptLaunchRunner.yaml +++ /dev/null @@ -1,9 +0,0 @@ -# the base provider -%YAML 1.1 ---- -name: PromptLaunchRunner -spec_type: provider -spec_version: 1.1 -description: "The capability provider for the prompt tools" -implements: std_capabilities2/PromptLaunchRunner -runner: launch/prompt/prompttool.launch.py diff --git a/std_capabilities2/providers/prompt/PromptOccupancyRunner.yaml b/std_capabilities2/providers/prompt/PromptOccupancyRunner.yaml deleted file mode 100644 index 25f1f63..0000000 --- a/std_capabilities2/providers/prompt/PromptOccupancyRunner.yaml +++ /dev/null @@ -1,12 +0,0 @@ -# the empty provider -%YAML 1.1 ---- -name: PromptOccupancyRunner -spec_type: provider -spec_version: 1.1 -description: "The capability provider for the prompting interface from prompt tools. This can be used for prompting the occupancy map around the robot." -implements: std_capabilities2/PromptOccupancyRunner -runner: capabilities2_runner::PromptOccupancyRunner -depends_on: - "std_capabilities2/PromptLaunchRunner": - provider: "std_capabilities2/PromptLaunchRunner" \ No newline at end of file diff --git a/std_capabilities2/providers/prompt/PromptPlanRequestRunner.yaml b/std_capabilities2/providers/prompt/PromptPlanRequestRunner.yaml deleted file mode 100644 index d83cffb..0000000 --- a/std_capabilities2/providers/prompt/PromptPlanRequestRunner.yaml +++ /dev/null @@ -1,13 +0,0 @@ -# the empty provider -%YAML 1.1 ---- -name: PromptPlanRequestRunner -spec_type: provider -spec_version: 1.1 -description: "The capability provider for the prompting for an execution plan. This can be used requesting a new plan at the beginning or when - new data is available" -implements: std_capabilities2/PromptPlanRequestRunner -runner: capabilities2_runner::PromptPlanRequestRunner -depends_on: - "std_capabilities2/PromptLaunchRunner": - provider: "std_capabilities2/PromptLaunchRunner" \ No newline at end of file diff --git a/std_capabilities2/providers/prompt/PromptPlanResponseRunner.yaml b/std_capabilities2/providers/prompt/PromptPlanResponseRunner.yaml deleted file mode 100644 index a7837b0..0000000 --- a/std_capabilities2/providers/prompt/PromptPlanResponseRunner.yaml +++ /dev/null @@ -1,12 +0,0 @@ -# the empty provider -%YAML 1.1 ---- -name: PromptPlanResponseRunner -spec_type: provider -spec_version: 1.1 -description: "The capability provider for executing the received plan. This can be used to trigger the execution process of the new plan" -implements: std_capabilities2/PromptPlanResponseRunner -runner: capabilities2_runner::PromptPlanResponseRunner -depends_on: - "std_capabilities2/PromptLaunchRunner": - provider: "std_capabilities2/PromptLaunchRunner" \ No newline at end of file diff --git a/std_capabilities2/providers/prompt/PromptPoseRunner.yaml b/std_capabilities2/providers/prompt/PromptPoseRunner.yaml deleted file mode 100644 index 5cdfccd..0000000 --- a/std_capabilities2/providers/prompt/PromptPoseRunner.yaml +++ /dev/null @@ -1,12 +0,0 @@ -# the empty provider -%YAML 1.1 ---- -name: PromptPoseRunner -spec_type: provider -spec_version: 1.1 -description: "The capability provider for the prompting interface from prompt tools. This can be used for prompting pose of the robot." -implements: std_capabilities2/PromptPoseRunner -runner: capabilities2_runner::PromptPoseRunner -depends_on: - "std_capabilities2/PromptLaunchRunner": - provider: "std_capabilities2/PromptLaunchRunner" \ No newline at end of file diff --git a/std_capabilities2/providers/prompt/PromptTextRunner.yaml b/std_capabilities2/providers/prompt/PromptTextRunner.yaml deleted file mode 100644 index 0cf0385..0000000 --- a/std_capabilities2/providers/prompt/PromptTextRunner.yaml +++ /dev/null @@ -1,12 +0,0 @@ -# the empty provider -%YAML 1.1 ---- -name: PromptTextRunner -spec_type: provider -spec_version: 1.1 -description: "The capability provider for the prompting interface from prompt tools. This can be used for prompting text." -implements: std_capabilities2/PromptTextRunner -runner: capabilities2_runner::PromptTextRunner -depends_on: - "std_capabilities2/PromptLaunchRunner": - provider: "std_capabilities2/PromptLaunchRunner" \ No newline at end of file From 34cdf982642e617ecd8cabd4af0077d677d4f664 Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Mon, 18 Nov 2024 13:43:29 +1100 Subject: [PATCH 035/133] added docker image --- .github/workflows/amd64_jazzy.yml | 67 ++++++++++++++++++++++++ docker/Dockerfile | 87 +++++++++++++++++++++++++++++++ docker/compose.yaml | 8 +++ docker/docs/startup.md | 29 +++++++++++ docker/workspace_entrypoint.sh | 8 +++ docs/nav2_setup.md | 4 +- readme.md | 1 + 7 files changed, 202 insertions(+), 2 deletions(-) create mode 100644 .github/workflows/amd64_jazzy.yml create mode 100644 docker/Dockerfile create mode 100644 docker/compose.yaml create mode 100644 docker/docs/startup.md create mode 100644 docker/workspace_entrypoint.sh diff --git a/.github/workflows/amd64_jazzy.yml b/.github/workflows/amd64_jazzy.yml new file mode 100644 index 0000000..1cddbcd --- /dev/null +++ b/.github/workflows/amd64_jazzy.yml @@ -0,0 +1,67 @@ +name: ROS Jazzy Capabilities2 + +on: + workflow_dispatch: + + push: + branches: + - capabilities2-server-fabric + paths-ignore: + - README.md + + pull_request: + branches: + - capabilities2-server-fabric + paths-ignore: + - README.md + +env: + REGISTRY: ghcr.io + OWNER: collaborativeroboticslab + IMAGE_NAME: capabilities2 + + +# https://docs.github.com/en/actions/using-jobs/using-concurrency +concurrency: + # only cancel in-progress jobs or runs for the current workflow - matches against branch & tags + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: true + +jobs: + + build-and-push-image: + runs-on: ubuntu-latest + + permissions: + contents: write + packages: write + + steps: + - name: Remove unnecessary files + run: | + sudo rm -rf /usr/share/dotnet + sudo rm -rf "$AGENT_TOOLSDIRECTORY" + + - name: Check out the repository + uses: actions/checkout@v4 + + - name: Set up QEMU + uses: docker/setup-qemu-action@v3 + + - name: Set up Docker Buildx + uses: docker/setup-buildx-action@v3 + + - name: Log in to the Container registry + run: | + docker login --username ${{ env.OWNER }} --password ${{ secrets.GH_PAT }} ghcr.io + + - name: Build and Push + uses: docker/build-push-action@v5 + with: + context: . + file: docker/Dockerfile + platforms: linux/amd64 + push: true + tags: | + ${{ env.REGISTRY }}/${{ env.OWNER }}/${{ env.IMAGE_NAME }}:latest + ${{ env.REGISTRY }}/${{ env.OWNER }}/${{ env.IMAGE_NAME }}:jazzy \ No newline at end of file diff --git a/docker/Dockerfile b/docker/Dockerfile new file mode 100644 index 0000000..5fb3f9c --- /dev/null +++ b/docker/Dockerfile @@ -0,0 +1,87 @@ +FROM ros:jazzy-ros-base-noble as base + +## Parameters +ENV WORKSPACE_ROOT=/capabilities2 + +############################################################################################################################# +##### +##### Install Dependencies +##### +############################################################################################################################# + +WORKDIR / + +RUN apt-get update -y +RUN apt-get install -y --no-install-recommends ros-dev-tools \ + ros-$ROS_DISTRO-tf2-eigen \ + ros-$ROS_DISTRO-rmw-cyclonedds-cpp \ + ros-$ROS_DISTRO-navigation2 \ + ros-$ROS_DISTRO-nav2-bringup \ + ros-$ROS_DISTRO-slam-toolbox + +RUN apt-get clean + + +############################################################################################################################# +##### +##### Clone packages and setup dependencies +##### +############################################################################################################################# + +WORKDIR ${WORKSPACE_ROOT}/src + +RUN git clone -b capabilities2-server-fabric https://github.com/CollaborativeRoboticsLab/capabilities2.git +RUN git clone https://github.com/AIResearchLab/nav_stack.git + +RUN rm /etc/ros/rosdep/sources.list.d/20-default.list + +RUN rosdep init && rosdep update && rosdep install --from-paths ${WORKSPACE_ROOT}/src -y --ignore-src + + +############################################################################################################################# +##### +##### Build Capabilities packages +##### +############################################################################################################################# + +WORKDIR ${WORKSPACE_ROOT} + +RUN . /opt/ros/humble/setup.sh && colcon build + +WORKDIR / + +COPY ${WORKSPACE_ROOT}/src/capabilities2/docker/workspace_entrypoint.sh /workspace_entrypoint.sh + +############################################################################################################################# +##### +##### Remove workspace source and build files that are not relevent to running the system +##### +############################################################################################################################# + +RUN rm -rf ${WORKSPACE_ROOT}/src +RUN rm -rf ${WORKSPACE_ROOT}/log +RUN rm -rf ${WORKSPACE_ROOT}/build + +RUN rm -rf /var/lib/apt/lists/* +RUN rm -rf /tmp/* +RUN apt-get clean + + +#--------------------------------------------------------------------------------------------------------------------------- +#---- +#---- Start final release image +#---- +#--------------------------------------------------------------------------------------------------------------------------- + + +FROM ros:jazzy-ros-base-noble as final + +## Parameters +ENV WORKSPACE_ROOT=/navstack +ENV RMW_IMPLEMENTATION=rmw_cyclonedds_cpp + +COPY --from=base / / + +RUN chmod +x /workspace_entrypoint.sh + +ENTRYPOINT [ "/workspace_entrypoint.sh" ] \ No newline at end of file diff --git a/docker/compose.yaml b/docker/compose.yaml new file mode 100644 index 0000000..8076dfa --- /dev/null +++ b/docker/compose.yaml @@ -0,0 +1,8 @@ +services: + navigation: + image: ghcr.io/collaborativeroboticslab/capabilities2:jazzy + restart: unless-stopped + privileged: true + network_mode: host + volumes: + - /dev:/dev \ No newline at end of file diff --git a/docker/docs/startup.md b/docker/docs/startup.md new file mode 100644 index 0000000..1a23eff --- /dev/null +++ b/docker/docs/startup.md @@ -0,0 +1,29 @@ +## Docker based startup information + +### Setup the Capabilities2 container + +Pull and start the docker container + +```bash +cd docker +docker compose pull +docker compose up -d +``` + +### Starting the Capabilities2 server + +On the host computer, same terminal or a new one + +```bash +docker exec -it capabilities2:jazzy bash +ros2 launch capabilities2_server server.launch.py +``` + +### Starting the Capabilities2 Fabric + +On the host computer, iIn a new terminal + +```bash +source install/setup.bash +ros2 launch capabilities2_fabric fabric.launch.py +``` diff --git a/docker/workspace_entrypoint.sh b/docker/workspace_entrypoint.sh new file mode 100644 index 0000000..fb3f777 --- /dev/null +++ b/docker/workspace_entrypoint.sh @@ -0,0 +1,8 @@ +#!/bin/bash +set -e + +# setup ros2 environment +source "/opt/ros/$ROS_DISTRO/setup.bash" +source "$WORKSPACE_ROOT/install/setup.bash" + +exec "$@" \ No newline at end of file diff --git a/docs/nav2_setup.md b/docs/nav2_setup.md index 47a9b12..1b27fe5 100644 --- a/docs/nav2_setup.md +++ b/docs/nav2_setup.md @@ -3,7 +3,7 @@ ## Install nav2 stack ```bash -sudo apt install ros-humble-navigation2 ros-humble-nav2-bringup +sudo apt install ros-$ROS_DISTRO-navigation2 ros-$ROS_DISTRO-nav2-bringup ros-$ROS_DISTRO-slam-toolbox ``` ## Clone configuration @@ -19,5 +19,5 @@ git clone https://github.com/CollaborativeRoboticsLab/nav_stack.git If using a simulated turtlebot3 for testing, install using following commands. ```bash -sudo apt install ros-humble-turtlebot3* +sudo apt install ros-$ROS_DISTRO-turtlebot3* ``` \ No newline at end of file diff --git a/readme.md b/readme.md index 157ac94..a5efcdf 100644 --- a/readme.md +++ b/readme.md @@ -58,6 +58,7 @@ ros2 launch capabilities2_fabric fabric.launch.py - [Registering a capability](./capabilities2_server/docs/register.md) - [Terminal based capability usage](./capabilities2_server/docs/terminal_usage.md) - [Running test scripts](./docs/run_test_scripts.md) +- [Using with Docker](./docker/docs/startup.md) ## Acknowledgements From 01faad932ad112259940dcea8b2ca2fa4ea6d1b9 Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Mon, 18 Nov 2024 16:36:13 +1100 Subject: [PATCH 036/133] added docker file --- docker/Dockerfile | 11 ++++++----- docker/compose.yaml | 2 +- docker/docs/startup.md | 4 ++-- docs/setup.md | 6 ++++-- docs/setup_with_dev.md | 4 +++- 5 files changed, 16 insertions(+), 11 deletions(-) diff --git a/docker/Dockerfile b/docker/Dockerfile index 5fb3f9c..b152ef6 100644 --- a/docker/Dockerfile +++ b/docker/Dockerfile @@ -31,7 +31,8 @@ RUN apt-get clean WORKDIR ${WORKSPACE_ROOT}/src RUN git clone -b capabilities2-server-fabric https://github.com/CollaborativeRoboticsLab/capabilities2.git -RUN git clone https://github.com/AIResearchLab/nav_stack.git +RUN git clone -b develop https://github.com/CollaborativeRoboticsLab/nav_stack.git +RUN git clone https://github.com/CollaborativeRoboticsLab/nav_stack.git RUN rm /etc/ros/rosdep/sources.list.d/20-default.list @@ -46,12 +47,10 @@ RUN rosdep init && rosdep update && rosdep install --from-paths ${WORKSPACE_ROOT WORKDIR ${WORKSPACE_ROOT} -RUN . /opt/ros/humble/setup.sh && colcon build +RUN . /opt/ros/jazzy/setup.sh && colcon build WORKDIR / -COPY ${WORKSPACE_ROOT}/src/capabilities2/docker/workspace_entrypoint.sh /workspace_entrypoint.sh - ############################################################################################################################# ##### ##### Remove workspace source and build files that are not relevent to running the system @@ -77,11 +76,13 @@ RUN apt-get clean FROM ros:jazzy-ros-base-noble as final ## Parameters -ENV WORKSPACE_ROOT=/navstack +ENV WORKSPACE_ROOT=/capabilities2 ENV RMW_IMPLEMENTATION=rmw_cyclonedds_cpp COPY --from=base / / +COPY docker/workspace_entrypoint.sh /workspace_entrypoint.sh + RUN chmod +x /workspace_entrypoint.sh ENTRYPOINT [ "/workspace_entrypoint.sh" ] \ No newline at end of file diff --git a/docker/compose.yaml b/docker/compose.yaml index 8076dfa..0ee4eea 100644 --- a/docker/compose.yaml +++ b/docker/compose.yaml @@ -1,5 +1,5 @@ services: - navigation: + capabilities2: image: ghcr.io/collaborativeroboticslab/capabilities2:jazzy restart: unless-stopped privileged: true diff --git a/docker/docs/startup.md b/docker/docs/startup.md index 1a23eff..7557655 100644 --- a/docker/docs/startup.md +++ b/docker/docs/startup.md @@ -5,7 +5,7 @@ Pull and start the docker container ```bash -cd docker +cd src/capabilities2/docker docker compose pull docker compose up -d ``` @@ -15,7 +15,7 @@ docker compose up -d On the host computer, same terminal or a new one ```bash -docker exec -it capabilities2:jazzy bash +docker run -it capabilities2 bash ros2 launch capabilities2_server server.launch.py ``` diff --git a/docs/setup.md b/docs/setup.md index 4378c37..a3de251 100644 --- a/docs/setup.md +++ b/docs/setup.md @@ -11,12 +11,14 @@ mkdir -p /home/$USER/capabilities_ws/src cd /home/$USER/capabilities_ws/src ``` -### Cloning the Package +### Cloning the Packages Clone the package using Git ```bash -git clone -b develop https://github.com/CollaborativeRoboticsLab/capabilities2.git +git clone -b capabilities2-server-fabric https://github.com/CollaborativeRoboticsLab/capabilities2.git +git clone -b develop https://github.com/CollaborativeRoboticsLab/nav_stack.git +git clone https://github.com/AIResearchLab/nav_stack.git ``` ### Dependency installation diff --git a/docs/setup_with_dev.md b/docs/setup_with_dev.md index 2d4be30..145d4ae 100644 --- a/docs/setup_with_dev.md +++ b/docs/setup_with_dev.md @@ -16,7 +16,9 @@ cd /home/$USER/capabilities_ws/src Clone the package using Git ```bash -git clone -b develop https://github.com/CollaborativeRoboticsLab/capabilities2.git +git clone -b capabilities2-server-fabric https://github.com/CollaborativeRoboticsLab/capabilities2.git +git clone -b develop https://github.com/CollaborativeRoboticsLab/nav_stack.git +git clone https://github.com/AIResearchLab/nav_stack.git ``` ### Devcontainer From c982eec159decf44e93712034f0dbae3e98caf14 Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Tue, 19 Nov 2024 11:34:41 +1100 Subject: [PATCH 037/133] Update setup.md --- docs/setup.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/docs/setup.md b/docs/setup.md index a3de251..736481e 100644 --- a/docs/setup.md +++ b/docs/setup.md @@ -17,7 +17,7 @@ Clone the package using Git ```bash git clone -b capabilities2-server-fabric https://github.com/CollaborativeRoboticsLab/capabilities2.git -git clone -b develop https://github.com/CollaborativeRoboticsLab/nav_stack.git +git clone -b develop https://github.com/CollaborativeRoboticsLab/std_capabilities.git git clone https://github.com/AIResearchLab/nav_stack.git ``` @@ -38,4 +38,4 @@ Use colcon to build the packages: ```bash colcon build -``` \ No newline at end of file +``` From 3f56fe86d4b94851e668e038f4f7ca4b4a08467f Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Tue, 19 Nov 2024 11:47:52 +1100 Subject: [PATCH 038/133] Update CMakeLists.txt --- capabilities2_runner/CMakeLists.txt | 1 - 1 file changed, 1 deletion(-) diff --git a/capabilities2_runner/CMakeLists.txt b/capabilities2_runner/CMakeLists.txt index 8d885d0..e6bbfe7 100644 --- a/capabilities2_runner/CMakeLists.txt +++ b/capabilities2_runner/CMakeLists.txt @@ -42,7 +42,6 @@ ament_target_dependencies(${PROJECT_NAME} rclcpp_action pluginlib capabilities2_msgs - TINYXML2 ) pluginlib_export_plugin_description_file(${PROJECT_NAME} plugins.xml) From 74dd47291a45f1913fe65f5a7c466cde6a176f37 Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Tue, 19 Nov 2024 11:49:34 +1100 Subject: [PATCH 039/133] Update CMakeLists.txt --- capabilities2_runner_bt/CMakeLists.txt | 1 - 1 file changed, 1 deletion(-) diff --git a/capabilities2_runner_bt/CMakeLists.txt b/capabilities2_runner_bt/CMakeLists.txt index 7cfd988..bcae60c 100644 --- a/capabilities2_runner_bt/CMakeLists.txt +++ b/capabilities2_runner_bt/CMakeLists.txt @@ -44,7 +44,6 @@ ament_target_dependencies(${PROJECT_NAME} capabilities2_msgs capabilities2_runner behaviortree_cpp - TINYXML2 ) pluginlib_export_plugin_description_file(capabilities2_runner plugins.xml) From b47de038babd7fac3b22a482a95c4628376d996f Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Tue, 19 Nov 2024 11:50:36 +1100 Subject: [PATCH 040/133] Update CMakeLists.txt --- capabilities2_runner_audio/CMakeLists.txt | 1 - 1 file changed, 1 deletion(-) diff --git a/capabilities2_runner_audio/CMakeLists.txt b/capabilities2_runner_audio/CMakeLists.txt index 4a1013e..9c39a62 100644 --- a/capabilities2_runner_audio/CMakeLists.txt +++ b/capabilities2_runner_audio/CMakeLists.txt @@ -45,7 +45,6 @@ ament_target_dependencies(${PROJECT_NAME} hri_audio_msgs capabilities2_msgs capabilities2_runner - TINYXML2 ) pluginlib_export_plugin_description_file(capabilities2_runner plugins.xml) From e2ad7dfbf26922899e63b7f7e031a13a6839e559 Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Tue, 19 Nov 2024 11:50:54 +1100 Subject: [PATCH 041/133] Update CMakeLists.txt --- capabilities2_runner_nav2/CMakeLists.txt | 1 - 1 file changed, 1 deletion(-) diff --git a/capabilities2_runner_nav2/CMakeLists.txt b/capabilities2_runner_nav2/CMakeLists.txt index ef912de..3edafbc 100644 --- a/capabilities2_runner_nav2/CMakeLists.txt +++ b/capabilities2_runner_nav2/CMakeLists.txt @@ -46,7 +46,6 @@ ament_target_dependencies(${PROJECT_NAME} geometry_msgs capabilities2_msgs capabilities2_runner - TINYXML2 ) pluginlib_export_plugin_description_file(capabilities2_runner plugins.xml) From bc46b277d56fc0c9dbf7bb0d27f88beacc4e6453 Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Tue, 19 Nov 2024 11:51:08 +1100 Subject: [PATCH 042/133] Update CMakeLists.txt --- capabilities2_runner_prompt/CMakeLists.txt | 1 - 1 file changed, 1 deletion(-) diff --git a/capabilities2_runner_prompt/CMakeLists.txt b/capabilities2_runner_prompt/CMakeLists.txt index e82abd8..ab76761 100644 --- a/capabilities2_runner_prompt/CMakeLists.txt +++ b/capabilities2_runner_prompt/CMakeLists.txt @@ -44,7 +44,6 @@ ament_target_dependencies(${PROJECT_NAME} prompt_msgs capabilities2_msgs capabilities2_runner - TINYXML2 ) pluginlib_export_plugin_description_file(capabilities2_runner plugins.xml) From b6945acf0ee77c3718cd2c370ac8b98bc6d5b95d Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Tue, 19 Nov 2024 11:52:32 +1100 Subject: [PATCH 043/133] Update CMakeLists.txt --- capabilities2_server/CMakeLists.txt | 1 - 1 file changed, 1 deletion(-) diff --git a/capabilities2_server/CMakeLists.txt b/capabilities2_server/CMakeLists.txt index b74f2a0..cb79c7f 100644 --- a/capabilities2_server/CMakeLists.txt +++ b/capabilities2_server/CMakeLists.txt @@ -64,7 +64,6 @@ ament_target_dependencies(${PROJECT_NAME} capabilities2_runner SQLite3 yaml-cpp - TINYXML2 UUID ) From 315c04d118336f2592c5ebbda9f44060b76537e9 Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Tue, 19 Nov 2024 21:26:44 +1100 Subject: [PATCH 044/133] fixed race conditions in capabilities2_fabric --- .../capabilities_fabric.hpp | 179 +++++++++++------- capabilities2_fabric/plans/default.xml | 12 +- capabilities2_fabric/plans/navigation_1.xml | 2 +- 3 files changed, 121 insertions(+), 72 deletions(-) diff --git a/capabilities2_fabric/include/capabilities2_executor/capabilities_fabric.hpp b/capabilities2_fabric/include/capabilities2_executor/capabilities_fabric.hpp index 26f443a..be7ca33 100644 --- a/capabilities2_fabric/include/capabilities2_executor/capabilities_fabric.hpp +++ b/capabilities2_fabric/include/capabilities2_executor/capabilities_fabric.hpp @@ -194,7 +194,6 @@ class CapabilitiesFabric : public rclcpp::Node */ void handle_accepted(const std::shared_ptr goal_handle) { - RCLCPP_INFO(this->get_logger(), "Execution started"); execution(goal_handle); } @@ -203,9 +202,14 @@ class CapabilitiesFabric : public rclcpp::Node */ void execution(const std::shared_ptr goal_handle) { + RCLCPP_INFO(this->get_logger(), "Execution started"); + expected_providers_ = 0; completed_providers_ = 0; + expected_interfaces_ = 0; + completed_interfaces_ = 0; + getInterfaces(goal_handle); } @@ -215,6 +219,7 @@ class CapabilitiesFabric : public rclcpp::Node void getInterfaces(const std::shared_ptr goal_handle) { auto feedback = std::make_shared(); + feedback->progress = "Requesting Interface information"; goal_handle->publish_feedback(feedback); @@ -242,78 +247,93 @@ class CapabilitiesFabric : public rclcpp::Node auto response = future.get(); RCLCPP_INFO(this->get_logger(), "Received Interface information"); - // Process each interface and get Semantic interfaces - for (const auto& interface : response->interfaces) - { - getSemanticInterfaces(interface, goal_handle); - } + expected_interfaces_ = response->interfaces.size(); + + RCLCPP_INFO(this->get_logger(), "Requsting Semantic Interface information for %d interfaces", expected_interfaces_); + + // Request each interface recursively for Semantic interfaces + getSemanticInterfaces(response->interfaces, goal_handle); }); } /** * @brief Get Semantic Interfaces */ - void getSemanticInterfaces(const std::string& interface, const std::shared_ptr goal_handle) + void getSemanticInterfaces(const std::vector& interfaces, const std::shared_ptr goal_handle) { auto feedback = std::make_shared(); - feedback->progress = "Requesting semantic interfaces for " + interface; + + std::string requested_interface = interfaces[completed_interfaces_]; + + RCLCPP_INFO(this->get_logger(), ""); + feedback->progress = "Requesting semantic interfaces for " + requested_interface; goal_handle->publish_feedback(feedback); RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); - auto request_sematic = std::make_shared(); - request_sematic->interface = interface; + auto request_semantic = std::make_shared(); + request_semantic->interface = requested_interface; // request semantic interface from the server auto result_semantic_future = get_sem_interf_client_->async_send_request( - request_sematic, [this, goal_handle, feedback, interface](GetSemanticInterfacesClient::SharedFuture future) { + request_semantic, [this, goal_handle, feedback, interfaces, requested_interface](GetSemanticInterfacesClient::SharedFuture future) { if (!future.valid()) { - RCLCPP_ERROR(this->get_logger(), "Failed to get Semantic Interface information"); - auto result = std::make_shared(); result->success = false; result->message = "Failed to get Semantic Interface information"; goal_handle->abort(result); - RCLCPP_INFO(this->get_logger(), "Server Execution Cancelled"); + RCLCPP_INFO(this->get_logger(), "Failed to get Semantic Interface information. Server Execution Cancelled"); return; } + completed_interfaces_++; auto response = future.get(); + // if semantic interfaces are availble for a given interface, add the semantic interface if (response->semantic_interfaces.size() > 0) { for (const auto& semantic_interface : response->semantic_interfaces) { - feedback->progress = "Received Semantic Interface information for " + interface + " as " + semantic_interface; + interface_list.push_back(semantic_interface); + is_semantic_list.push_back(true); + + feedback->progress = std::to_string(completed_interfaces_) + "/" + std::to_string(expected_interfaces_) + " : Received " + + semantic_interface + " for " + requested_interface + ". So added " + semantic_interface; goal_handle->publish_feedback(feedback); RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); - - interface_list_sem.push_back(semantic_interface); } } // if no semantic interfaces are availble for a given interface, add the interface instead else { - feedback->progress = "No Semantic Interface information for " + interface; + interface_list.push_back(requested_interface); + is_semantic_list.push_back(false); + + feedback->progress = std::to_string(completed_interfaces_) + "/" + std::to_string(expected_interfaces_) + " : Received none for " + + requested_interface + ". So added " + requested_interface; goal_handle->publish_feedback(feedback); - RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); - interface_list.push_back(interface); + RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); } - expected_providers_ = interface_list.size() + interface_list_sem.size(); - - for (const auto& interface : interface_list) + if (completed_interfaces_ != expected_interfaces_) { - getProvider(interface, goal_handle, false); + // Request next interface recursively for Semantic interfaces + getSemanticInterfaces(interfaces, goal_handle); } - - for (const auto& sem_interface : interface_list_sem) + else { - interface_list.push_back(sem_interface); - getProvider(interface, goal_handle, true); + RCLCPP_INFO(this->get_logger(), ""); + RCLCPP_INFO(this->get_logger(), "Received all requested Interface information"); + + expected_providers_ = interface_list.size(); + + RCLCPP_INFO(this->get_logger(), "Requsting Provider information for %d providers", expected_providers_); + + // request providers from the interfaces in the interfaces_list + getProvider(interface_list, is_semantic_list, goal_handle); } }); } @@ -322,40 +342,51 @@ class CapabilitiesFabric : public rclcpp::Node * @brief Get Providers * */ - void getProvider(const std::string& interface, const std::shared_ptr goal_handle, bool include_semantic) + void getProvider(const std::vector& interfaces, const std::vector& is_semantic, + const std::shared_ptr goal_handle) { + std::string requested_interface = interfaces[completed_providers_]; + bool semantic_flag = is_semantic[completed_providers_]; + auto feedback = std::make_shared(); - feedback->progress = "Requesting provider for " + interface; + + RCLCPP_INFO(this->get_logger(), ""); + feedback->progress = "Requesting provider for " + requested_interface; goal_handle->publish_feedback(feedback); RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); auto request_providers = std::make_shared(); // request providers of the semantic interface - request_providers->interface = interface; - request_providers->include_semantic = include_semantic; + request_providers->interface = requested_interface; + request_providers->include_semantic = semantic_flag; auto result_providers_future = get_providers_client_->async_send_request( - request_providers, [this, interface, goal_handle, feedback](GetProvidersClient::SharedFuture future) { + request_providers, [this, is_semantic, requested_interface, interfaces, goal_handle, feedback](GetProvidersClient::SharedFuture future) { if (!future.valid()) { - RCLCPP_ERROR(this->get_logger(), "Failed to receive provider for interface: %s", interface.c_str()); - auto result = std::make_shared(); result->success = false; - result->message = "Failed to retrieve providers for interface: " + interface; - + result->message = "Did not retrieve providers for interface: " + requested_interface; goal_handle->abort(result); + + RCLCPP_ERROR(this->get_logger(), result->message.c_str()); return; } + completed_providers_++; auto response = future.get(); - RCLCPP_INFO(this->get_logger(), "Received Providers for Interface: %s", interface.c_str()); - completed_providers_++; + if (response->default_provider != "") + { + // add defualt provider to the list + providers_list.push_back(response->default_provider); - // add defualt provider to the list - providers_list.push_back(response->default_provider); + feedback->progress = std::to_string(completed_providers_) + "/" + std::to_string(expected_providers_) + " : Received " + + response->default_provider + " for " + requested_interface + ". So added " + response->default_provider; + goal_handle->publish_feedback(feedback); + RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); + } // add additional providers to the list if available if (response->providers.size() > 0) @@ -363,13 +394,31 @@ class CapabilitiesFabric : public rclcpp::Node for (const auto& provider : response->providers) { providers_list.push_back(provider); + feedback->progress = std::to_string(completed_providers_) + "/" + std::to_string(expected_providers_) + " : Received and added " + + provider + " for " + requested_interface; + goal_handle->publish_feedback(feedback); + RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); } } + else + { + feedback->progress = std::to_string(completed_providers_) + "/" + std::to_string(expected_providers_) + " : No providers for " + + requested_interface; + goal_handle->publish_feedback(feedback); + RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); + } // Check if all expected calls are completed before calling verify_plan - if (completed_providers_ == expected_providers_) + if (completed_providers_ != expected_providers_) + { + // request providers for the next interface in the interfaces_list + getProvider(interfaces, is_semantic, goal_handle); + } + else { auto feedback = std::make_shared(); + + RCLCPP_INFO(this->get_logger(), ""); feedback->progress = "All requested interface, semantic interface and provider data recieved"; goal_handle->publish_feedback(feedback); RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); @@ -537,15 +586,16 @@ class CapabilitiesFabric : public rclcpp::Node goal_handle->publish_feedback(feedback); RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); + expected_capabilities_ = connection_map.size(); + // start all runners and interfaces that the connections depend on for (const auto& [key, value] : connection_map) { + RCLCPP_INFO(this->get_logger(), ""); feedback->progress = "Starting capability of Node " + std::to_string(key) + " named " + value.source.runner; goal_handle->publish_feedback(feedback); RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); - expected_capabilities_++; - use_capability(value.source.runner, value.source.provider, goal_handle); } }); @@ -561,9 +611,6 @@ class CapabilitiesFabric : public rclcpp::Node void use_capability(const std::string& capability, const std::string& provider, const std::shared_ptr goal_handle) { auto feedback = std::make_shared(); - feedback->progress = "Using capability " + capability + " from " + provider; - goal_handle->publish_feedback(feedback); - RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); auto request_use = std::make_shared(); request_use->capability = capability; @@ -588,13 +635,14 @@ class CapabilitiesFabric : public rclcpp::Node completed_capabilities_++; auto response = future.get(); - feedback->progress = "Successfully used capability " + capability + " from " + provider; + feedback->progress = "Successfully started capability " + capability + " from " + provider; goal_handle->publish_feedback(feedback); RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); // Check if all expected calls are completed before calling verify_plan if (completed_capabilities_ == expected_capabilities_) { + RCLCPP_INFO(this->get_logger(), ""); feedback->progress = "All requested capabilities have started"; goal_handle->publish_feedback(feedback); RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); @@ -665,6 +713,7 @@ class CapabilitiesFabric : public rclcpp::Node { auto request_configure = std::make_shared(); + RCLCPP_INFO(this->get_logger(), ""); feedback->progress = "Configuring capability of Node " + std::to_string(key) + " named " + value.source.runner; goal_handle->publish_feedback(feedback); RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); @@ -674,9 +723,8 @@ class CapabilitiesFabric : public rclcpp::Node request_configure->source.capability = value.source.runner; request_configure->source.provider = value.source.provider; - RCLCPP_INFO(this->get_logger(), "Source Capability : %s", request_configure->source.capability.c_str()); - RCLCPP_INFO(this->get_logger(), "Source Provider : %s", request_configure->source.provider.c_str()); - RCLCPP_INFO(this->get_logger(), "Source Parameters : %s", request_configure->source.parameters.c_str()); + RCLCPP_INFO(this->get_logger(), "Source capability : %s, provider : %s", request_configure->source.capability.c_str(), + request_configure->source.provider.c_str()); } else { @@ -689,9 +737,8 @@ class CapabilitiesFabric : public rclcpp::Node request_configure->target_on_start.capability = value.target_on_start.runner; request_configure->target_on_start.provider = value.target_on_start.provider; - RCLCPP_INFO(this->get_logger(), "Triggered on start Capability : %s", request_configure->target_on_start.capability.c_str()); - RCLCPP_INFO(this->get_logger(), "Triggered on start Provider : %s", request_configure->target_on_start.provider.c_str()); - RCLCPP_INFO(this->get_logger(), "Triggered on start Parameters : %s", request_configure->target_on_start.parameters.c_str()); + RCLCPP_INFO(this->get_logger(), "Triggered on start capability : %s provider : %s", request_configure->target_on_start.capability.c_str(), + request_configure->target_on_start.provider.c_str()); } else { @@ -704,9 +751,8 @@ class CapabilitiesFabric : public rclcpp::Node request_configure->target_on_stop.capability = value.target_on_stop.runner; request_configure->target_on_stop.provider = value.target_on_stop.provider; - RCLCPP_INFO(this->get_logger(), "Triggered on stop Capability : %s", request_configure->target_on_stop.capability.c_str()); - RCLCPP_INFO(this->get_logger(), "Triggered on stop Provider : %s", request_configure->target_on_stop.provider.c_str()); - RCLCPP_INFO(this->get_logger(), "Triggered on stop Parameters : %s", request_configure->target_on_stop.parameters.c_str()); + RCLCPP_INFO(this->get_logger(), "Triggered on stop capability : %s provider : %s", request_configure->target_on_stop.capability.c_str(), + request_configure->target_on_stop.provider.c_str()); } else { @@ -719,9 +765,8 @@ class CapabilitiesFabric : public rclcpp::Node request_configure->target_on_success.capability = value.target_on_success.runner; request_configure->target_on_success.provider = value.target_on_success.provider; - RCLCPP_INFO(this->get_logger(), "Triggered on success Capability : %s", request_configure->target_on_success.capability.c_str()); - RCLCPP_INFO(this->get_logger(), "Triggered on success Provider : %s", request_configure->target_on_success.provider.c_str()); - RCLCPP_INFO(this->get_logger(), "Triggered on success Parameters : %s", request_configure->target_on_success.parameters.c_str()); + RCLCPP_INFO(this->get_logger(), "Triggered on success capability : %s provider : %s", request_configure->target_on_success.capability.c_str(), + request_configure->target_on_success.provider.c_str()); } else { @@ -734,9 +779,8 @@ class CapabilitiesFabric : public rclcpp::Node request_configure->target_on_failure.capability = value.target_on_failure.runner; request_configure->target_on_failure.provider = value.target_on_failure.provider; - RCLCPP_INFO(this->get_logger(), "Triggered on failure Capability : %s", request_configure->target_on_failure.capability.c_str()); - RCLCPP_INFO(this->get_logger(), "Triggered on failure Provider : %s", request_configure->target_on_failure.provider.c_str()); - RCLCPP_INFO(this->get_logger(), "Triggered on failure Parameters : %s", request_configure->target_on_failure.parameters.c_str()); + RCLCPP_INFO(this->get_logger(), "Triggered on failure capability : %s provider : %s", request_configure->target_on_failure.capability.c_str(), + request_configure->target_on_failure.provider.c_str()); } else { @@ -764,17 +808,19 @@ class CapabilitiesFabric : public rclcpp::Node completed_configurations_++; auto response = future.get(); - feedback->progress = "Successfully configure capability :" + value.source.runner; + feedback->progress = "Successfully configured capability :" + value.source.runner; goal_handle->publish_feedback(feedback); RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); // Check if all expected calls are completed before calling verify_plan if (completed_configurations_ == expected_configurations_) { + RCLCPP_INFO(this->get_logger(), ""); feedback->progress = "All requested capabilities have been configured"; goal_handle->publish_feedback(feedback); RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); + RCLCPP_INFO(this->get_logger(), ""); feedback->progress = "Triggering the first capability"; goal_handle->publish_feedback(feedback); RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); @@ -827,6 +873,9 @@ class CapabilitiesFabric : public rclcpp::Node /** flag to select loading from file or accepting via action server */ bool read_file; + int expected_interfaces_; + int completed_interfaces_; + int expected_providers_; int completed_providers_; @@ -846,7 +895,7 @@ class CapabilitiesFabric : public rclcpp::Node std::map connection_map; /** Interface List */ - std::vector interface_list_sem; + std::vector is_semantic_list; /** Interface List */ std::vector interface_list; diff --git a/capabilities2_fabric/plans/default.xml b/capabilities2_fabric/plans/default.xml index cbcb513..40fdfd9 100644 --- a/capabilities2_fabric/plans/default.xml +++ b/capabilities2_fabric/plans/default.xml @@ -1,11 +1,11 @@ - - - - - - + + + + + + \ No newline at end of file diff --git a/capabilities2_fabric/plans/navigation_1.xml b/capabilities2_fabric/plans/navigation_1.xml index 52012c1..7e48c39 100644 --- a/capabilities2_fabric/plans/navigation_1.xml +++ b/capabilities2_fabric/plans/navigation_1.xml @@ -1,6 +1,6 @@ - + \ No newline at end of file From 4363822eac669f0ca949d729b43e6023cc6b17c2 Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Tue, 19 Nov 2024 22:41:30 +1100 Subject: [PATCH 045/133] updated launch runner --- .../capabilities2_runner/launch_runner.hpp | 26 +++++++++++++++---- 1 file changed, 21 insertions(+), 5 deletions(-) diff --git a/capabilities2_runner/include/capabilities2_runner/launch_runner.hpp b/capabilities2_runner/include/capabilities2_runner/launch_runner.hpp index 3c41180..0191a3a 100644 --- a/capabilities2_runner/include/capabilities2_runner/launch_runner.hpp +++ b/capabilities2_runner/include/capabilities2_runner/launch_runner.hpp @@ -49,16 +49,23 @@ class LaunchRunner : public RunnerBase { // Child process start execlp("/bin/bash", "/bin/bash", "-c", command.c_str(), NULL); - perror("execlp"); // If execlp fails - exit(1); + perror("execlp failed to execute ROS 2 launch file"); // If execlp fails + exit(EXIT_FAILURE); // Child process end } if (childPid == -1) - RCLCPP_ERROR(node_->get_logger(), "%s launch file from %s failed", launch_name.c_str(), package_name.c_str()); + { + std::string error_msg = "Failed to start " + launch_name + " from " + package_name; + RCLCPP_ERROR(node_->get_logger(), error_msg.c_str()); + throw std::runtime_error(error_msg); + } else - RCLCPP_INFO(node_->get_logger(), "%s launch file from %s started with PID : %d", launch_name.c_str(), - package_name.c_str(), childPid); + { + std::string info_msg = + "Started " + launch_name + " from " + package_name + " with PID : " + std::to_string(childPid); + RCLCPP_INFO(node_->get_logger(), info_msg.c_str()); + } } /** @@ -74,6 +81,15 @@ class LaunchRunner : public RunnerBase RCLCPP_INFO(node_->get_logger(), "%s launch file from %s stopped : %d", launch_name.c_str(), package_name.c_str()); childPid = -1; + + if (kill(childPid, SIGTERM) == 0) + { + if (waitpid(childPid, NULL, 0) != childPid) + { + RCLCPP_WARN(node_->get_logger(), "%s in process %d did not terminate; sending SIGKILL", launch_name.c_str(), childPid); + kill(childPid, SIGKILL); + } + } } } From 14fe88027331032da9ae8bffdb184c381c38c921 Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Wed, 20 Nov 2024 01:11:52 +1100 Subject: [PATCH 046/133] added seperate launch for launch files --- capabilities2_launch/.clang-format | 64 +++++++++ capabilities2_launch/CMakeLists.txt | 31 +++++ .../capabilities2_launch/launch_server.hpp | 101 ++++++++++++++ capabilities2_launch/package.xml | 21 +++ capabilities2_launch/src/launch_server.cpp | 18 +++ capabilities2_msgs/CMakeLists.txt | 2 + capabilities2_msgs/srv/LaunchStart.srv | 6 + capabilities2_msgs/srv/LaunchStop.srv | 4 + .../capabilities2_runner/launch_runner.hpp | 124 +++++++++++------- capabilities2_server/launch/server.launch.py | 6 +- 10 files changed, 330 insertions(+), 47 deletions(-) create mode 100644 capabilities2_launch/.clang-format create mode 100644 capabilities2_launch/CMakeLists.txt create mode 100644 capabilities2_launch/include/capabilities2_launch/launch_server.hpp create mode 100644 capabilities2_launch/package.xml create mode 100644 capabilities2_launch/src/launch_server.cpp create mode 100644 capabilities2_msgs/srv/LaunchStart.srv create mode 100644 capabilities2_msgs/srv/LaunchStop.srv diff --git a/capabilities2_launch/.clang-format b/capabilities2_launch/.clang-format new file mode 100644 index 0000000..d36804f --- /dev/null +++ b/capabilities2_launch/.clang-format @@ -0,0 +1,64 @@ + +BasedOnStyle: Google +AccessModifierOffset: -2 +ConstructorInitializerIndentWidth: 2 +AlignEscapedNewlinesLeft: false +AlignTrailingComments: true +AllowAllParametersOfDeclarationOnNextLine: false +AllowShortIfStatementsOnASingleLine: false +AllowShortLoopsOnASingleLine: false +AllowShortFunctionsOnASingleLine: None +AlwaysBreakTemplateDeclarations: true +AlwaysBreakBeforeMultilineStrings: false +BreakBeforeBinaryOperators: false +BreakBeforeTernaryOperators: false +BreakConstructorInitializersBeforeComma: true +BinPackParameters: true +ColumnLimit: 120 +ConstructorInitializerAllOnOneLineOrOnePerLine: true +DerivePointerBinding: false +PointerBindsToType: true +ExperimentalAutoDetectBinPacking: false +IndentCaseLabels: true +MaxEmptyLinesToKeep: 1 +NamespaceIndentation: None +ObjCSpaceBeforeProtocolList: true +PenaltyBreakBeforeFirstCallParameter: 19 +PenaltyBreakComment: 60 +PenaltyBreakString: 1 +PenaltyBreakFirstLessLess: 1000 +PenaltyExcessCharacter: 1000 +PenaltyReturnTypeOnItsOwnLine: 90 +SpacesBeforeTrailingComments: 2 +Cpp11BracedListStyle: false +Standard: Auto +IndentWidth: 2 +TabWidth: 2 +UseTab: Never +IndentFunctionDeclarationAfterType: false +SpacesInParentheses: false +SpacesInAngles: false +SpaceInEmptyParentheses: false +SpacesInCStyleCastParentheses: false +SpaceAfterControlStatementKeyword: true +SpaceBeforeAssignmentOperators: true +ContinuationIndentWidth: 4 +SortIncludes: false +SpaceAfterCStyleCast: false + +# Configure each individual brace in BraceWrapping +BreakBeforeBraces: Custom + +# Control of individual brace wrapping cases +BraceWrapping: { + AfterClass: 'true' + AfterControlStatement: 'true' + AfterEnum : 'true' + AfterFunction : 'true' + AfterNamespace : 'true' + AfterStruct : 'true' + AfterUnion : 'true' + BeforeCatch : 'true' + BeforeElse : 'true' + IndentBraces : 'false' +} diff --git a/capabilities2_launch/CMakeLists.txt b/capabilities2_launch/CMakeLists.txt new file mode 100644 index 0000000..c3649b2 --- /dev/null +++ b/capabilities2_launch/CMakeLists.txt @@ -0,0 +1,31 @@ +cmake_minimum_required(VERSION 3.8) +project(capabilities2_launch) + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(capabilities2_msgs REQUIRED) + +include_directories( + include +) + +add_executable(${PROJECT_NAME} + src/launch_server.cpp +) + +ament_target_dependencies(${PROJECT_NAME} + rclcpp + capabilities2_msgs +) + +install(TARGETS + ${PROJECT_NAME} + DESTINATION lib/${PROJECT_NAME} +) + +install(DIRECTORY include/ + DESTINATION include +) + +ament_package() diff --git a/capabilities2_launch/include/capabilities2_launch/launch_server.hpp b/capabilities2_launch/include/capabilities2_launch/launch_server.hpp new file mode 100644 index 0000000..115af11 --- /dev/null +++ b/capabilities2_launch/include/capabilities2_launch/launch_server.hpp @@ -0,0 +1,101 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +class LaunchServer : public rclcpp::Node +{ +public: + LaunchServer() : Node("Launch_File_Server") + { + start_server_ = this->create_service( + "/capabilities/launch_start", + std::bind(&LaunchServer::start_request, this, std::placeholders::_1, std::placeholders::_2)); + + stop_server_ = this->create_service( + "/capabilities/launch_stop", + std::bind(&LaunchServer::stop_request, this, std::placeholders::_1, std::placeholders::_2)); + + RCLCPP_INFO(this->get_logger(), "LaunchFileServer is ready."); + } + +private: + // Callback function to handle requests + void start_request(const std::shared_ptr request, + const std::shared_ptr response) + { + package_name = request->package_name; + launch_name = request->launch_file_name; + + RCLCPP_INFO(this->get_logger(), "Received request: package = %s, launch file = %s", package_name.c_str(), + launch_name.c_str()); + + std::string command = "source install/setup.bash && ros2 launch " + package_name + " " + launch_name; + + int childPid = fork(); + + if (childPid == 0) + { + // Child process start + execlp("/bin/bash", "/bin/bash", "-c", command.c_str(), NULL); + perror("execlp failed to execute ROS 2 launch file"); // If execlp fails + exit(EXIT_FAILURE); + // Child process end + } + + if (childPid == -1) + { + std::string error_msg = "Failed to start " + launch_name + " from " + package_name; + RCLCPP_ERROR(this->get_logger(), error_msg.c_str()); + throw std::runtime_error(error_msg); + } + else + { + std::string info_msg = + "Started " + launch_name + " from " + package_name + " with PID : " + std::to_string(childPid); + RCLCPP_INFO(this->get_logger(), info_msg.c_str()); + } + + response->pid = childPid; + } + + void stop_request(const std::shared_ptr request, + const std::shared_ptr response) + { + pid = request->pid; + + RCLCPP_INFO(this->get_logger(), "Received request: stopping PID = %d", pid); + + if (pid != -1) + { + if (kill(pid, SIGTERM) == 0) + { + if (waitpid(pid, NULL, 0) != pid) + { + RCLCPP_WARN(this->get_logger(), "%s in process %d did not terminate; sending SIGKILL", launch_name.c_str(), + pid); + kill(pid, SIGKILL); + } + } + } + } + + // Service server + rclcpp::Service::SharedPtr start_server_; + rclcpp::Service::SharedPtr stop_server_; + + std::string launch_name; + std::string package_name; + + int pid; +}; \ No newline at end of file diff --git a/capabilities2_launch/package.xml b/capabilities2_launch/package.xml new file mode 100644 index 0000000..61a2ec1 --- /dev/null +++ b/capabilities2_launch/package.xml @@ -0,0 +1,21 @@ + + + + capabilities2_launch + 0.0.0 + TODO: Package description + kalana + TODO: License declaration + + ament_cmake + + rclcpp + capabilities2_msgs + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/capabilities2_launch/src/launch_server.cpp b/capabilities2_launch/src/launch_server.cpp new file mode 100644 index 0000000..4b7b2a1 --- /dev/null +++ b/capabilities2_launch/src/launch_server.cpp @@ -0,0 +1,18 @@ +#include +#include + +int main(int argc, char **argv) +{ + // Initialize the ROS 2 system + rclcpp::init(argc, argv); + + // Create an instance of the server node + auto node = std::make_shared(); + + // Spin the node to process requests + rclcpp::spin(node); + + // Shutdown ROS 2 + rclcpp::shutdown(); + return 0; +} \ No newline at end of file diff --git a/capabilities2_msgs/CMakeLists.txt b/capabilities2_msgs/CMakeLists.txt index 5ea912d..10e0ec8 100644 --- a/capabilities2_msgs/CMakeLists.txt +++ b/capabilities2_msgs/CMakeLists.txt @@ -42,6 +42,8 @@ set(srv_files "srv/UseCapability.srv" "srv/ConfigureCapability.srv" "srv/TriggerCapability.srv" + "srv/LaunchStart.srv" + "srv/LaunchStop.srv" ) set(action_files diff --git a/capabilities2_msgs/srv/LaunchStart.srv b/capabilities2_msgs/srv/LaunchStart.srv new file mode 100644 index 0000000..84a96e8 --- /dev/null +++ b/capabilities2_msgs/srv/LaunchStart.srv @@ -0,0 +1,6 @@ +std_msgs/Header header +string package_name +string launch_file_name +--- +std_msgs/Header header +int32 pid \ No newline at end of file diff --git a/capabilities2_msgs/srv/LaunchStop.srv b/capabilities2_msgs/srv/LaunchStop.srv new file mode 100644 index 0000000..5615899 --- /dev/null +++ b/capabilities2_msgs/srv/LaunchStop.srv @@ -0,0 +1,4 @@ +std_msgs/Header header +int32 pid +--- +std_msgs/Header header \ No newline at end of file diff --git a/capabilities2_runner/include/capabilities2_runner/launch_runner.hpp b/capabilities2_runner/include/capabilities2_runner/launch_runner.hpp index 0191a3a..db53fd3 100644 --- a/capabilities2_runner/include/capabilities2_runner/launch_runner.hpp +++ b/capabilities2_runner/include/capabilities2_runner/launch_runner.hpp @@ -1,14 +1,8 @@ #pragma once -#include #include -#include -#include -#include -#include -#include -#include -#include +#include +#include namespace capabilities2_runner { @@ -21,6 +15,9 @@ namespace capabilities2_runner class LaunchRunner : public RunnerBase { public: + using LaunchStart = capabilities2_msgs::srv::LaunchStart; + using LaunchStop = capabilities2_msgs::srv::LaunchStop; + /** * @brief Constructor which needs to be empty due to plugin semantics */ @@ -41,31 +38,61 @@ class LaunchRunner : public RunnerBase package_name = run_config_.runner.substr(0, run_config_.runner.find("/")); launch_name = run_config_.runner.substr(run_config_.runner.find("/") + 1); - std::string command = "source install/setup.bash && ros2 launch " + package_name + " " + launch_name; + // create an service client + start_service_client_ = node_->create_client("/capabilities/launch_start"); - int childPid = fork(); + RCLCPP_INFO(node_->get_logger(), "%s waiting for service: /capabilities/launch_start", + run_config_.interface.c_str()); - if (childPid == 0) + if (!start_service_client_->wait_for_service(std::chrono::seconds(3))) { - // Child process start - execlp("/bin/bash", "/bin/bash", "-c", command.c_str(), NULL); - perror("execlp failed to execute ROS 2 launch file"); // If execlp fails - exit(EXIT_FAILURE); - // Child process end + RCLCPP_ERROR(node_->get_logger(), "%s failed to connect to service: /capabilities/launch_start", + run_config_.interface.c_str()); + throw runner_exception("Failed to connect to server: /capabilities/launch_start"); } - if (childPid == -1) - { - std::string error_msg = "Failed to start " + launch_name + " from " + package_name; - RCLCPP_ERROR(node_->get_logger(), error_msg.c_str()); - throw std::runtime_error(error_msg); - } - else + RCLCPP_INFO(node_->get_logger(), "%s connected to service: /capabilities/launch_start", + run_config_.interface.c_str()); + + // create an service client + stop_service_client_ = node_->create_client("/capabilities/launch_stop"); + + // wait for action server + RCLCPP_INFO(node_->get_logger(), "%s waiting for service: /capabilities/launch_stop", + run_config_.interface.c_str()); + + if (!stop_service_client_->wait_for_service(std::chrono::seconds(3))) { - std::string info_msg = - "Started " + launch_name + " from " + package_name + " with PID : " + std::to_string(childPid); - RCLCPP_INFO(node_->get_logger(), info_msg.c_str()); + RCLCPP_ERROR(node_->get_logger(), "%s failed to connect to service: /capabilities/launch_stop", + run_config_.interface.c_str()); + throw runner_exception("Failed to connect to server: /capabilities/launch_stop"); } + + RCLCPP_INFO(node_->get_logger(), "%s connected to service: /capabilities/launch_stop", + run_config_.interface.c_str()); + + // generate a reequest from launch_name and package_name + auto request_msg = std::make_shared(); + + request_msg->package_name = package_name; + request_msg->launch_file_name = launch_name; + + RCLCPP_INFO(node_->get_logger(), "Requesting to launch %s from %s", launch_name.c_str(), package_name.c_str()); + + auto result_future = start_service_client_->async_send_request( + request_msg, [this](typename rclcpp::Client::SharedFuture future) { + if (!future.valid()) + { + RCLCPP_ERROR(node_->get_logger(), "Request to launch %s from %s failed", launch_name.c_str(), package_name.c_str()); + return; + } + + RCLCPP_INFO(node_->get_logger(), "Request to launch %s from %s succeeded", launch_name.c_str(), package_name.c_str()); + + auto response = future.get(); + + childPid = response->pid; + }); } /** @@ -74,23 +101,29 @@ class LaunchRunner : public RunnerBase */ virtual void stop() override { - if (childPid != -1) - { - kill(childPid, SIGTERM); // Send termination signal to child - waitpid(childPid, NULL, 0); // Wait for child to terminate - RCLCPP_INFO(node_->get_logger(), "%s launch file from %s stopped : %d", launch_name.c_str(), - package_name.c_str()); - childPid = -1; - - if (kill(childPid, SIGTERM) == 0) - { - if (waitpid(childPid, NULL, 0) != childPid) - { - RCLCPP_WARN(node_->get_logger(), "%s in process %d did not terminate; sending SIGKILL", launch_name.c_str(), childPid); - kill(childPid, SIGKILL); - } - } - } + // 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"); + + // generate a reequest from launch_name and package_name + auto request_msg = std::make_shared(); + + request_msg->pid = childPid; + + RCLCPP_INFO(node_->get_logger(), "Requesting to stop %s with PID %d", launch_name.c_str(), childPid); + + auto result_future = stop_service_client_->async_send_request( + request_msg, [this](typename rclcpp::Client::SharedFuture future) { + if (!future.valid()) + { + RCLCPP_ERROR(node_->get_logger(), "Request to stop %s from %s failed ", launch_name.c_str(), package_name.c_str()); + return; + } + + RCLCPP_INFO(node_->get_logger(), "Request to launch %s from %s succeeded ", launch_name.c_str(), package_name.c_str()); + }); } // throw on trigger function @@ -99,9 +132,12 @@ class LaunchRunner : public RunnerBase throw runner_exception("No Trigger as this is launch runner"); } - pid_t childPid = -1; + int childPid = -1; std::string launch_name; std::string package_name; + + rclcpp::Client::SharedPtr start_service_client_; + rclcpp::Client::SharedPtr stop_service_client_; }; } // namespace capabilities2_runner \ No newline at end of file diff --git a/capabilities2_server/launch/server.launch.py b/capabilities2_server/launch/server.launch.py index ebe6b6a..7b865e5 100644 --- a/capabilities2_server/launch/server.launch.py +++ b/capabilities2_server/launch/server.launch.py @@ -37,9 +37,9 @@ def generate_launch_description(): # create launch proxy node launch_proxy = Node( - package='capabilities2_launch_proxy', - executable='capabilities_launch_proxy', - name='capabilities_launch_proxy' + package='capabilities2_launch', + executable='capabilities2_launch', + name='capabilities2_launch' ) # return From e9777eaf79dec7cc53c3473984751bda11066785 Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Wed, 20 Nov 2024 01:54:24 +1100 Subject: [PATCH 047/133] added launch package in python --- .../capabilities2_launch/launch_server.hpp | 39 +++----- .../capabilities2_launch_py/__init__.py | 0 .../capabilities2_launch_py/launch_server.py | 95 +++++++++++++++++++ capabilities2_launch_py/package.xml | 21 ++++ .../resource/capabilities2_launch_py | 0 capabilities2_launch_py/setup.cfg | 4 + capabilities2_launch_py/setup.py | 26 +++++ .../test/test_copyright.py | 25 +++++ capabilities2_launch_py/test/test_flake8.py | 25 +++++ capabilities2_launch_py/test/test_pep257.py | 23 +++++ capabilities2_server/launch/server.launch.py | 6 +- 11 files changed, 235 insertions(+), 29 deletions(-) create mode 100644 capabilities2_launch_py/capabilities2_launch_py/__init__.py create mode 100644 capabilities2_launch_py/capabilities2_launch_py/launch_server.py create mode 100644 capabilities2_launch_py/package.xml create mode 100644 capabilities2_launch_py/resource/capabilities2_launch_py create mode 100644 capabilities2_launch_py/setup.cfg create mode 100644 capabilities2_launch_py/setup.py create mode 100644 capabilities2_launch_py/test/test_copyright.py create mode 100644 capabilities2_launch_py/test/test_flake8.py create mode 100644 capabilities2_launch_py/test/test_pep257.py diff --git a/capabilities2_launch/include/capabilities2_launch/launch_server.hpp b/capabilities2_launch/include/capabilities2_launch/launch_server.hpp index 115af11..1566ddb 100644 --- a/capabilities2_launch/include/capabilities2_launch/launch_server.hpp +++ b/capabilities2_launch/include/capabilities2_launch/launch_server.hpp @@ -40,33 +40,21 @@ class LaunchServer : public rclcpp::Node RCLCPP_INFO(this->get_logger(), "Received request: package = %s, launch file = %s", package_name.c_str(), launch_name.c_str()); - std::string command = "source install/setup.bash && ros2 launch " + package_name + " " + launch_name; + std::string command = "/bin/bash -c \"source install/setup.bash && ros2 launch " + package_name + " " + launch_name + "\""; - int childPid = fork(); - - if (childPid == 0) - { - // Child process start - execlp("/bin/bash", "/bin/bash", "-c", command.c_str(), NULL); - perror("execlp failed to execute ROS 2 launch file"); // If execlp fails - exit(EXIT_FAILURE); - // Child process end - } - - if (childPid == -1) + // Execute the command using system() + int status = system(command.c_str()); + if (status == -1) { std::string error_msg = "Failed to start " + launch_name + " from " + package_name; RCLCPP_ERROR(this->get_logger(), error_msg.c_str()); throw std::runtime_error(error_msg); } - else - { - std::string info_msg = - "Started " + launch_name + " from " + package_name + " with PID : " + std::to_string(childPid); - RCLCPP_INFO(this->get_logger(), info_msg.c_str()); - } - response->pid = childPid; + // Since `system()` doesn't return the PID directly, consider improving the mechanism if PID tracking is essential. + pid = getpid(); // This is just an example and may need improvement depending on the need. + + response->pid = pid; } void stop_request(const std::shared_ptr request, @@ -80,12 +68,11 @@ class LaunchServer : public rclcpp::Node { if (kill(pid, SIGTERM) == 0) { - if (waitpid(pid, NULL, 0) != pid) - { - RCLCPP_WARN(this->get_logger(), "%s in process %d did not terminate; sending SIGKILL", launch_name.c_str(), - pid); - kill(pid, SIGKILL); - } + RCLCPP_INFO(this->get_logger(), "Sent SIGTERM to PID %d", pid); + } + else + { + RCLCPP_WARN(this->get_logger(), "Failed to terminate PID %d with SIGTERM", pid); } } } diff --git a/capabilities2_launch_py/capabilities2_launch_py/__init__.py b/capabilities2_launch_py/capabilities2_launch_py/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/capabilities2_launch_py/capabilities2_launch_py/launch_server.py b/capabilities2_launch_py/capabilities2_launch_py/launch_server.py new file mode 100644 index 0000000..414af38 --- /dev/null +++ b/capabilities2_launch_py/capabilities2_launch_py/launch_server.py @@ -0,0 +1,95 @@ +import os +import signal +import subprocess +import rclpy +from rclpy.node import Node +from capabilities2_msgs.srv import LaunchStart, LaunchStop + +DETACHED_PROCESS = 0x00000008 + +class LaunchServer(Node): + def __init__(self): + super().__init__('launch_file_server') + + # Service for starting a launch file + self.start_server = self.create_service( + LaunchStart, + '/capabilities/launch_start', + self.start_request + ) + + # Service for stopping a launch file + self.stop_server = self.create_service( + LaunchStop, + '/capabilities/launch_stop', + self.stop_request + ) + + # Dictionary to track running processes + self.processes = {} + + self.get_logger().info('LaunchFileServer is ready.') + + def start_request(self, request, response): + package_name = request.package_name + launch_name = request.launch_file_name + + self.get_logger().info(f'Received request: package = {package_name}, launch file = {launch_name}') + + # Construct the command to execute + command = f'/bin/bash -c "source install/setup.bash && ros2 launch {package_name} {launch_name}"' + + # Start the process + try: + process = subprocess.Popen(command, shell=True, preexec_fn=os.setsid, close_fds=True, creationflags=DETACHED_PROCESS) + self.processes[process.pid] = process # Track the process + + self.get_logger().info(f'Started {launch_name} from {package_name} with PID: {process.pid}') + response.pid = process.pid + except Exception as e: + self.get_logger().error(f'Failed to start {launch_name} from {package_name}: {str(e)}') + response.pid = -1 + + return response + + def stop_request(self, request, response): + pid = request.pid + self.get_logger().info(f'Received request: stopping PID = {pid}') + + if pid in self.processes: + try: + # Kill the process group to stop all child processes + os.killpg(os.getpgid(pid), signal.SIGTERM) + self.processes[pid].wait() # Wait for the process to terminate + del self.processes[pid] + self.get_logger().info(f'Successfully stopped process with PID: {pid}') + except Exception as e: + self.get_logger().warn(f'Failed to terminate PID {pid}: {str(e)}') + else: + self.get_logger().warn(f'No process found with PID {pid}') + + return response + + +def main(args=None): + rclpy.init(args=args) + + # Create the LaunchServer node + launch_server = LaunchServer() + + # Keep the node running + try: + rclpy.spin(launch_server) + except KeyboardInterrupt: + launch_server.get_logger().info('Shutting down LaunchFileServer...') + finally: + # Clean up all running processes + for pid, process in launch_server.processes.items(): + os.killpg(os.getpgid(pid), signal.SIGTERM) + process.wait() + + rclpy.shutdown() + + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/capabilities2_launch_py/package.xml b/capabilities2_launch_py/package.xml new file mode 100644 index 0000000..5660d5d --- /dev/null +++ b/capabilities2_launch_py/package.xml @@ -0,0 +1,21 @@ + + + + capabilities2_launch_py + 0.0.0 + TODO: Package description + kalana + TODO: License declaration + + rclcpp + capabilities2_msgs + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/capabilities2_launch_py/resource/capabilities2_launch_py b/capabilities2_launch_py/resource/capabilities2_launch_py new file mode 100644 index 0000000..e69de29 diff --git a/capabilities2_launch_py/setup.cfg b/capabilities2_launch_py/setup.cfg new file mode 100644 index 0000000..5cc0c4a --- /dev/null +++ b/capabilities2_launch_py/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/capabilities2_launch_py +[install] +install_scripts=$base/lib/capabilities2_launch_py diff --git a/capabilities2_launch_py/setup.py b/capabilities2_launch_py/setup.py new file mode 100644 index 0000000..1427806 --- /dev/null +++ b/capabilities2_launch_py/setup.py @@ -0,0 +1,26 @@ +from setuptools import find_packages, setup + +package_name = 'capabilities2_launch_py' + +setup( + name=package_name, + version='0.1.0', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='kalana', + maintainer_email='kalanaratnayake95@gmail.com', + description='A ROS 2 package to manage starting and stopping ROS 2 launch files through services.', + license='TODO: License declaration', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'capabilities2_launch_py = capabilities2_launch_py.launch_server:main', + ], + }, +) diff --git a/capabilities2_launch_py/test/test_copyright.py b/capabilities2_launch_py/test/test_copyright.py new file mode 100644 index 0000000..97a3919 --- /dev/null +++ b/capabilities2_launch_py/test/test_copyright.py @@ -0,0 +1,25 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_copyright.main import main +import pytest + + +# Remove the `skip` decorator once the source file(s) have a copyright header +@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/capabilities2_launch_py/test/test_flake8.py b/capabilities2_launch_py/test/test_flake8.py new file mode 100644 index 0000000..27ee107 --- /dev/null +++ b/capabilities2_launch_py/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, \ + 'Found %d code style errors / warnings:\n' % len(errors) + \ + '\n'.join(errors) diff --git a/capabilities2_launch_py/test/test_pep257.py b/capabilities2_launch_py/test/test_pep257.py new file mode 100644 index 0000000..b234a38 --- /dev/null +++ b/capabilities2_launch_py/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found code style errors / warnings' diff --git a/capabilities2_server/launch/server.launch.py b/capabilities2_server/launch/server.launch.py index 7b865e5..f424544 100644 --- a/capabilities2_server/launch/server.launch.py +++ b/capabilities2_server/launch/server.launch.py @@ -37,9 +37,9 @@ def generate_launch_description(): # create launch proxy node launch_proxy = Node( - package='capabilities2_launch', - executable='capabilities2_launch', - name='capabilities2_launch' + package='capabilities2_launch_py', + executable='capabilities2_launch_py', + name='capabilities2_launch_py' ) # return From 89f87e8acbc47c74a47796dd724d67c46011d6a2 Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Wed, 20 Nov 2024 02:28:29 +1100 Subject: [PATCH 048/133] minor change --- .../capabilities2_launch_py/launch_server.py | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/capabilities2_launch_py/capabilities2_launch_py/launch_server.py b/capabilities2_launch_py/capabilities2_launch_py/launch_server.py index 414af38..23cacbb 100644 --- a/capabilities2_launch_py/capabilities2_launch_py/launch_server.py +++ b/capabilities2_launch_py/capabilities2_launch_py/launch_server.py @@ -2,11 +2,10 @@ import signal import subprocess import rclpy +import copy from rclpy.node import Node from capabilities2_msgs.srv import LaunchStart, LaunchStop -DETACHED_PROCESS = 0x00000008 - class LaunchServer(Node): def __init__(self): super().__init__('launch_file_server') @@ -41,7 +40,9 @@ def start_request(self, request, response): # Start the process try: - process = subprocess.Popen(command, shell=True, preexec_fn=os.setsid, close_fds=True, creationflags=DETACHED_PROCESS) + env = copy.deepcopy(os.environ) + env['PYTHONUNBUFFERED'] = 'x' + process = subprocess.Popen(command, shell=True, preexec_fn=os.setsid, close_fds=True, stdout=subprocess.PIPE, stderr=subprocess.STDOUT, env=env) self.processes[process.pid] = process # Track the process self.get_logger().info(f'Started {launch_name} from {package_name} with PID: {process.pid}') From 03e6ca2e353261bd73e7ee01143bedc8b9584f81 Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Thu, 21 Nov 2024 02:01:23 +1100 Subject: [PATCH 049/133] added user name and password --- .github/workflows/amd64_jazzy.yml | 5 ++++- docker/Dockerfile | 9 ++++++--- 2 files changed, 10 insertions(+), 4 deletions(-) diff --git a/.github/workflows/amd64_jazzy.yml b/.github/workflows/amd64_jazzy.yml index 1cddbcd..17a2287 100644 --- a/.github/workflows/amd64_jazzy.yml +++ b/.github/workflows/amd64_jazzy.yml @@ -64,4 +64,7 @@ jobs: push: true tags: | ${{ env.REGISTRY }}/${{ env.OWNER }}/${{ env.IMAGE_NAME }}:latest - ${{ env.REGISTRY }}/${{ env.OWNER }}/${{ env.IMAGE_NAME }}:jazzy \ No newline at end of file + ${{ env.REGISTRY }}/${{ env.OWNER }}/${{ env.IMAGE_NAME }}:jazzy + build-args: | + GITHUB_USERNAME=${{ github.actor }} + GITHUB_PAT=${{ secrets.GH_PAT }} \ No newline at end of file diff --git a/docker/Dockerfile b/docker/Dockerfile index b152ef6..c68929d 100644 --- a/docker/Dockerfile +++ b/docker/Dockerfile @@ -3,6 +3,9 @@ FROM ros:jazzy-ros-base-noble as base ## Parameters ENV WORKSPACE_ROOT=/capabilities2 +ARG GITHUB_USERNAME +ARG GITHUB_PAT + ############################################################################################################################# ##### ##### Install Dependencies @@ -30,9 +33,9 @@ RUN apt-get clean WORKDIR ${WORKSPACE_ROOT}/src -RUN git clone -b capabilities2-server-fabric https://github.com/CollaborativeRoboticsLab/capabilities2.git -RUN git clone -b develop https://github.com/CollaborativeRoboticsLab/nav_stack.git -RUN git clone https://github.com/CollaborativeRoboticsLab/nav_stack.git +RUN git clone -b capabilities2-server-fabric https://${GITHUB_USERNAME}:${GITHUB_PAT}@github.com/CollaborativeRoboticsLab/capabilities2.git +RUN git clone -b develop https://${GITHUB_USERNAME}:${GITHUB_PAT}@github.com/CollaborativeRoboticsLab/nav_stack.git +RUN git clone https://${GITHUB_USERNAME}:${GITHUB_PAT}@github.com/CollaborativeRoboticsLab/nav_stack.git RUN rm /etc/ros/rosdep/sources.list.d/20-default.list From b4c980be708d9afec69cc758029f63615308bd96 Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Thu, 21 Nov 2024 02:06:39 +1100 Subject: [PATCH 050/133] added user name and password --- docker/Dockerfile | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/docker/Dockerfile b/docker/Dockerfile index c68929d..337c743 100644 --- a/docker/Dockerfile +++ b/docker/Dockerfile @@ -24,6 +24,11 @@ RUN apt-get install -y --no-install-recommends ros-dev-tools \ RUN apt-get clean +RUN git clone https://github.com/leethomason/tinyxml2.git + +WORKDIR /tinyxml2 +RUN make install + ############################################################################################################################# ##### @@ -34,7 +39,7 @@ RUN apt-get clean WORKDIR ${WORKSPACE_ROOT}/src RUN git clone -b capabilities2-server-fabric https://${GITHUB_USERNAME}:${GITHUB_PAT}@github.com/CollaborativeRoboticsLab/capabilities2.git -RUN git clone -b develop https://${GITHUB_USERNAME}:${GITHUB_PAT}@github.com/CollaborativeRoboticsLab/nav_stack.git +RUN git clone -b develop https://${GITHUB_USERNAME}:${GITHUB_PAT}@github.com/CollaborativeRoboticsLab/std_capabilities.git RUN git clone https://${GITHUB_USERNAME}:${GITHUB_PAT}@github.com/CollaborativeRoboticsLab/nav_stack.git RUN rm /etc/ros/rosdep/sources.list.d/20-default.list From cf81a028def81ca38bf7932e42d5f8b5a60842b2 Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Thu, 21 Nov 2024 02:13:14 +1100 Subject: [PATCH 051/133] added user name and password --- docker/Dockerfile | 2 ++ 1 file changed, 2 insertions(+) diff --git a/docker/Dockerfile b/docker/Dockerfile index 337c743..598972a 100644 --- a/docker/Dockerfile +++ b/docker/Dockerfile @@ -41,6 +41,8 @@ WORKDIR ${WORKSPACE_ROOT}/src RUN git clone -b capabilities2-server-fabric https://${GITHUB_USERNAME}:${GITHUB_PAT}@github.com/CollaborativeRoboticsLab/capabilities2.git RUN git clone -b develop https://${GITHUB_USERNAME}:${GITHUB_PAT}@github.com/CollaborativeRoboticsLab/std_capabilities.git RUN git clone https://${GITHUB_USERNAME}:${GITHUB_PAT}@github.com/CollaborativeRoboticsLab/nav_stack.git +RUN git clone -b ros2/devel https://${GITHUB_USERNAME}:${GITHUB_PAT}@github.com/CollaborativeRoboticsLab/prompt_tools.git +RUN git clone https://github.com/CollaborativeRoboticsLab/hri_audio_msgs.git RUN rm /etc/ros/rosdep/sources.list.d/20-default.list From 1fd3d17917c754d8167167255114e8b2ecabcce9 Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Thu, 21 Nov 2024 03:17:56 +1100 Subject: [PATCH 052/133] minor fix --- docker/Dockerfile | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/docker/Dockerfile b/docker/Dockerfile index 598972a..dd8776a 100644 --- a/docker/Dockerfile +++ b/docker/Dockerfile @@ -95,4 +95,6 @@ COPY docker/workspace_entrypoint.sh /workspace_entrypoint.sh RUN chmod +x /workspace_entrypoint.sh -ENTRYPOINT [ "/workspace_entrypoint.sh" ] \ No newline at end of file +ENTRYPOINT [ "/workspace_entrypoint.sh" ] + +CMD ["/bin/bash"] \ No newline at end of file From 72b689352d8db52bdbf7c396ad775ad51c2dbf83 Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Thu, 21 Nov 2024 03:38:19 +1100 Subject: [PATCH 053/133] minor fix --- docker/Dockerfile | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docker/Dockerfile b/docker/Dockerfile index dd8776a..28202e2 100644 --- a/docker/Dockerfile +++ b/docker/Dockerfile @@ -97,4 +97,4 @@ RUN chmod +x /workspace_entrypoint.sh ENTRYPOINT [ "/workspace_entrypoint.sh" ] -CMD ["/bin/bash"] \ No newline at end of file +CMD ["bash"] \ No newline at end of file From 954c1634c5e8159a6ae5265caaaf8a70446297f3 Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Thu, 21 Nov 2024 11:09:17 +1100 Subject: [PATCH 054/133] minor fix --- docker/Dockerfile | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docker/Dockerfile b/docker/Dockerfile index 28202e2..7fec305 100644 --- a/docker/Dockerfile +++ b/docker/Dockerfile @@ -97,4 +97,4 @@ RUN chmod +x /workspace_entrypoint.sh ENTRYPOINT [ "/workspace_entrypoint.sh" ] -CMD ["bash"] \ No newline at end of file +CMD ["/bin/sh", "-c", "bash"] \ No newline at end of file From 3e968842a6718310407d4b54db5c1a79812ab0e4 Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Fri, 29 Nov 2024 17:43:28 +1100 Subject: [PATCH 055/133] added navstack launch to the main launch file as launching from launch_py or launch_proxy fails --- capabilities2_launch/.clang-format | 64 ------------- capabilities2_launch/CMakeLists.txt | 31 ------ .../capabilities2_launch/launch_server.hpp | 88 ----------------- capabilities2_launch/package.xml | 21 ---- capabilities2_launch/src/launch_server.cpp | 18 ---- .../capabilities2_launch_py/launch_server.py | 51 ++++------ .../launch_process/__init__.py | 0 .../launch_process/launch_process.py | 95 +++++++++++++++++++ capabilities2_launch_py/setup.py | 3 +- capabilities2_msgs/CMakeLists.txt | 3 +- .../srv/{LaunchStart.srv => Launch.srv} | 3 +- capabilities2_msgs/srv/LaunchStop.srv | 4 - .../capabilities2_runner/launch_runner.hpp | 32 +++---- capabilities2_server/launch/system.launch.py | 20 ++++ 14 files changed, 148 insertions(+), 285 deletions(-) delete mode 100644 capabilities2_launch/.clang-format delete mode 100644 capabilities2_launch/CMakeLists.txt delete mode 100644 capabilities2_launch/include/capabilities2_launch/launch_server.hpp delete mode 100644 capabilities2_launch/package.xml delete mode 100644 capabilities2_launch/src/launch_server.cpp create mode 100644 capabilities2_launch_py/launch_process/__init__.py create mode 100644 capabilities2_launch_py/launch_process/launch_process.py rename capabilities2_msgs/srv/{LaunchStart.srv => Launch.srv} (68%) delete mode 100644 capabilities2_msgs/srv/LaunchStop.srv create mode 100644 capabilities2_server/launch/system.launch.py diff --git a/capabilities2_launch/.clang-format b/capabilities2_launch/.clang-format deleted file mode 100644 index d36804f..0000000 --- a/capabilities2_launch/.clang-format +++ /dev/null @@ -1,64 +0,0 @@ - -BasedOnStyle: Google -AccessModifierOffset: -2 -ConstructorInitializerIndentWidth: 2 -AlignEscapedNewlinesLeft: false -AlignTrailingComments: true -AllowAllParametersOfDeclarationOnNextLine: false -AllowShortIfStatementsOnASingleLine: false -AllowShortLoopsOnASingleLine: false -AllowShortFunctionsOnASingleLine: None -AlwaysBreakTemplateDeclarations: true -AlwaysBreakBeforeMultilineStrings: false -BreakBeforeBinaryOperators: false -BreakBeforeTernaryOperators: false -BreakConstructorInitializersBeforeComma: true -BinPackParameters: true -ColumnLimit: 120 -ConstructorInitializerAllOnOneLineOrOnePerLine: true -DerivePointerBinding: false -PointerBindsToType: true -ExperimentalAutoDetectBinPacking: false -IndentCaseLabels: true -MaxEmptyLinesToKeep: 1 -NamespaceIndentation: None -ObjCSpaceBeforeProtocolList: true -PenaltyBreakBeforeFirstCallParameter: 19 -PenaltyBreakComment: 60 -PenaltyBreakString: 1 -PenaltyBreakFirstLessLess: 1000 -PenaltyExcessCharacter: 1000 -PenaltyReturnTypeOnItsOwnLine: 90 -SpacesBeforeTrailingComments: 2 -Cpp11BracedListStyle: false -Standard: Auto -IndentWidth: 2 -TabWidth: 2 -UseTab: Never -IndentFunctionDeclarationAfterType: false -SpacesInParentheses: false -SpacesInAngles: false -SpaceInEmptyParentheses: false -SpacesInCStyleCastParentheses: false -SpaceAfterControlStatementKeyword: true -SpaceBeforeAssignmentOperators: true -ContinuationIndentWidth: 4 -SortIncludes: false -SpaceAfterCStyleCast: false - -# Configure each individual brace in BraceWrapping -BreakBeforeBraces: Custom - -# Control of individual brace wrapping cases -BraceWrapping: { - AfterClass: 'true' - AfterControlStatement: 'true' - AfterEnum : 'true' - AfterFunction : 'true' - AfterNamespace : 'true' - AfterStruct : 'true' - AfterUnion : 'true' - BeforeCatch : 'true' - BeforeElse : 'true' - IndentBraces : 'false' -} diff --git a/capabilities2_launch/CMakeLists.txt b/capabilities2_launch/CMakeLists.txt deleted file mode 100644 index c3649b2..0000000 --- a/capabilities2_launch/CMakeLists.txt +++ /dev/null @@ -1,31 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(capabilities2_launch) - -# find dependencies -find_package(ament_cmake REQUIRED) -find_package(rclcpp REQUIRED) -find_package(capabilities2_msgs REQUIRED) - -include_directories( - include -) - -add_executable(${PROJECT_NAME} - src/launch_server.cpp -) - -ament_target_dependencies(${PROJECT_NAME} - rclcpp - capabilities2_msgs -) - -install(TARGETS - ${PROJECT_NAME} - DESTINATION lib/${PROJECT_NAME} -) - -install(DIRECTORY include/ - DESTINATION include -) - -ament_package() diff --git a/capabilities2_launch/include/capabilities2_launch/launch_server.hpp b/capabilities2_launch/include/capabilities2_launch/launch_server.hpp deleted file mode 100644 index 1566ddb..0000000 --- a/capabilities2_launch/include/capabilities2_launch/launch_server.hpp +++ /dev/null @@ -1,88 +0,0 @@ -#pragma once - -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include - -class LaunchServer : public rclcpp::Node -{ -public: - LaunchServer() : Node("Launch_File_Server") - { - start_server_ = this->create_service( - "/capabilities/launch_start", - std::bind(&LaunchServer::start_request, this, std::placeholders::_1, std::placeholders::_2)); - - stop_server_ = this->create_service( - "/capabilities/launch_stop", - std::bind(&LaunchServer::stop_request, this, std::placeholders::_1, std::placeholders::_2)); - - RCLCPP_INFO(this->get_logger(), "LaunchFileServer is ready."); - } - -private: - // Callback function to handle requests - void start_request(const std::shared_ptr request, - const std::shared_ptr response) - { - package_name = request->package_name; - launch_name = request->launch_file_name; - - RCLCPP_INFO(this->get_logger(), "Received request: package = %s, launch file = %s", package_name.c_str(), - launch_name.c_str()); - - std::string command = "/bin/bash -c \"source install/setup.bash && ros2 launch " + package_name + " " + launch_name + "\""; - - // Execute the command using system() - int status = system(command.c_str()); - if (status == -1) - { - std::string error_msg = "Failed to start " + launch_name + " from " + package_name; - RCLCPP_ERROR(this->get_logger(), error_msg.c_str()); - throw std::runtime_error(error_msg); - } - - // Since `system()` doesn't return the PID directly, consider improving the mechanism if PID tracking is essential. - pid = getpid(); // This is just an example and may need improvement depending on the need. - - response->pid = pid; - } - - void stop_request(const std::shared_ptr request, - const std::shared_ptr response) - { - pid = request->pid; - - RCLCPP_INFO(this->get_logger(), "Received request: stopping PID = %d", pid); - - if (pid != -1) - { - if (kill(pid, SIGTERM) == 0) - { - RCLCPP_INFO(this->get_logger(), "Sent SIGTERM to PID %d", pid); - } - else - { - RCLCPP_WARN(this->get_logger(), "Failed to terminate PID %d with SIGTERM", pid); - } - } - } - - // Service server - rclcpp::Service::SharedPtr start_server_; - rclcpp::Service::SharedPtr stop_server_; - - std::string launch_name; - std::string package_name; - - int pid; -}; \ No newline at end of file diff --git a/capabilities2_launch/package.xml b/capabilities2_launch/package.xml deleted file mode 100644 index 61a2ec1..0000000 --- a/capabilities2_launch/package.xml +++ /dev/null @@ -1,21 +0,0 @@ - - - - capabilities2_launch - 0.0.0 - TODO: Package description - kalana - TODO: License declaration - - ament_cmake - - rclcpp - capabilities2_msgs - - ament_lint_auto - ament_lint_common - - - ament_cmake - - diff --git a/capabilities2_launch/src/launch_server.cpp b/capabilities2_launch/src/launch_server.cpp deleted file mode 100644 index 4b7b2a1..0000000 --- a/capabilities2_launch/src/launch_server.cpp +++ /dev/null @@ -1,18 +0,0 @@ -#include -#include - -int main(int argc, char **argv) -{ - // Initialize the ROS 2 system - rclcpp::init(argc, argv); - - // Create an instance of the server node - auto node = std::make_shared(); - - // Spin the node to process requests - rclcpp::spin(node); - - // Shutdown ROS 2 - rclcpp::shutdown(); - return 0; -} \ No newline at end of file diff --git a/capabilities2_launch_py/capabilities2_launch_py/launch_server.py b/capabilities2_launch_py/capabilities2_launch_py/launch_server.py index 23cacbb..0f1a462 100644 --- a/capabilities2_launch_py/capabilities2_launch_py/launch_server.py +++ b/capabilities2_launch_py/capabilities2_launch_py/launch_server.py @@ -1,10 +1,9 @@ import os import signal -import subprocess import rclpy -import copy +from launch_process import launch_process from rclpy.node import Node -from capabilities2_msgs.srv import LaunchStart, LaunchStop +from capabilities2_msgs.srv import Launch class LaunchServer(Node): def __init__(self): @@ -12,14 +11,14 @@ def __init__(self): # Service for starting a launch file self.start_server = self.create_service( - LaunchStart, + Launch, '/capabilities/launch_start', self.start_request ) # Service for stopping a launch file self.stop_server = self.create_service( - LaunchStop, + Launch, '/capabilities/launch_stop', self.stop_request ) @@ -33,41 +32,25 @@ def start_request(self, request, response): package_name = request.package_name launch_name = request.launch_file_name - self.get_logger().info(f'Received request: package = {package_name}, launch file = {launch_name}') + self.get_logger().info(f'Received launch start request: package = {package_name}, launch file = {launch_name}') - # Construct the command to execute - command = f'/bin/bash -c "source install/setup.bash && ros2 launch {package_name} {launch_name}"' + name = package_name + "/" + launch_name - # Start the process - try: - env = copy.deepcopy(os.environ) - env['PYTHONUNBUFFERED'] = 'x' - process = subprocess.Popen(command, shell=True, preexec_fn=os.setsid, close_fds=True, stdout=subprocess.PIPE, stderr=subprocess.STDOUT, env=env) - self.processes[process.pid] = process # Track the process - - self.get_logger().info(f'Started {launch_name} from {package_name} with PID: {process.pid}') - response.pid = process.pid - except Exception as e: - self.get_logger().error(f'Failed to start {launch_name} from {package_name}: {str(e)}') - response.pid = -1 + self.processes[name] = launch_process.LaunchProcess(package_name=package_name, launch_file_name=launch_name) + self.processes[name].start() return response def stop_request(self, request, response): - pid = request.pid - self.get_logger().info(f'Received request: stopping PID = {pid}') - - if pid in self.processes: - try: - # Kill the process group to stop all child processes - os.killpg(os.getpgid(pid), signal.SIGTERM) - self.processes[pid].wait() # Wait for the process to terminate - del self.processes[pid] - self.get_logger().info(f'Successfully stopped process with PID: {pid}') - except Exception as e: - self.get_logger().warn(f'Failed to terminate PID {pid}: {str(e)}') - else: - self.get_logger().warn(f'No process found with PID {pid}') + package_name = request.package_name + launch_name = request.launch_file_name + + self.get_logger().info(f'Received launch stop request: package = {package_name}, launch file = {launch_name}') + + name = package_name + "/" + launch_name + + self.processes[name].stop() + self.processes[name].join() return response diff --git a/capabilities2_launch_py/launch_process/__init__.py b/capabilities2_launch_py/launch_process/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/capabilities2_launch_py/launch_process/launch_process.py b/capabilities2_launch_py/launch_process/launch_process.py new file mode 100644 index 0000000..14ba22d --- /dev/null +++ b/capabilities2_launch_py/launch_process/launch_process.py @@ -0,0 +1,95 @@ +from multiprocessing import Process +import launch +from ros2launch.api.api import parse_launch_arguments +from ros2launch.api import get_share_file_path_from_package +from ament_index_python.packages import PackageNotFoundError +from ros2launch.api import MultipleLaunchFilesError + +class LaunchProcess(Process): + + def __init__( + self, + package_name, + launch_file_name, + launch_file_arguments=[], + option_extensions={}, + noninteractive=True, + debug=False, + args=None + ): + """ + LaunchProcess objects represent ros2 launch files that need to be started is run in a separate process + + :param: package_name name of the package where launch file resides + :param: launch_file_name name of the launch file + :param: launch_file_arguments launch file arguments as a list + :param: option_extensions option extensions as a dictionary + :param: noninteractive flag for non-interactiveness + :param: debug flag for enabling debug info + :param: args additional arguments + """ + super().__init__() + self.package_name = package_name + self.launch_file_name = launch_file_name + self.launch_file_arguments = launch_file_arguments + self.noninteractive = noninteractive + self.debug = debug + self.option_extensions=option_extensions + self.args = args + self.path = None + + try: + self.path = get_share_file_path_from_package( + package_name=self.package_name, + file_name=self.launch_file_name) + + except PackageNotFoundError as exc: + raise RuntimeError(f"Package '{self.package_name}' not found: {exc}") + except (FileNotFoundError, MultipleLaunchFilesError) as exc: + raise RuntimeError(str(exc)) + + def run(self): + print(f"Launch file {self.launch_file_name} from package {self.package_name} started.") + + launch_service = launch.LaunchService( + argv=self.launch_file_arguments, + noninteractive=self.noninteractive, + debug=self.debug) + + parsed_launch_arguments = parse_launch_arguments(self.launch_file_arguments) + + launch_description = launch.LaunchDescription([ + launch.actions.IncludeLaunchDescription( + launch.launch_description_sources.AnyLaunchDescriptionSource( + self.path + ), + launch_arguments=parsed_launch_arguments, + ), + ]) + + for name in sorted(self.option_extensions.keys()): + result = self.option_extensions[name].prelaunch(launch_description, self.args) + launch_description = result[0] + + launch_service.include_launch_description(launch_description) + + launch_service.run() + + def stop(self): + print(f"Stopping Launch file {self.launch_file_name} from package {self.package_name}") + self.kill() + +if __name__ == "__main__": + launch_process = LaunchProcess( + package_name="nav_stack", + launch_file_name="system.launch.py" + ) + + launch_process.start() # Start the process + + import time + time.sleep(10) # Let the process run for 10 seconds + + launch_process.stop() # Signal the process to stop + launch_process.join() # Wait for the process to terminate + print("Launch process has exited.") \ No newline at end of file diff --git a/capabilities2_launch_py/setup.py b/capabilities2_launch_py/setup.py index 1427806..717f564 100644 --- a/capabilities2_launch_py/setup.py +++ b/capabilities2_launch_py/setup.py @@ -7,8 +7,7 @@ version='0.1.0', packages=find_packages(exclude=['test']), data_files=[ - ('share/ament_index/resource_index/packages', - ['resource/' + package_name]), + ('share/ament_index/resource_index/packages', ['resource/' + package_name]), ('share/' + package_name, ['package.xml']), ], install_requires=['setuptools'], diff --git a/capabilities2_msgs/CMakeLists.txt b/capabilities2_msgs/CMakeLists.txt index 10e0ec8..58edbe3 100644 --- a/capabilities2_msgs/CMakeLists.txt +++ b/capabilities2_msgs/CMakeLists.txt @@ -42,8 +42,7 @@ set(srv_files "srv/UseCapability.srv" "srv/ConfigureCapability.srv" "srv/TriggerCapability.srv" - "srv/LaunchStart.srv" - "srv/LaunchStop.srv" + "srv/Launch.srv" ) set(action_files diff --git a/capabilities2_msgs/srv/LaunchStart.srv b/capabilities2_msgs/srv/Launch.srv similarity index 68% rename from capabilities2_msgs/srv/LaunchStart.srv rename to capabilities2_msgs/srv/Launch.srv index 84a96e8..b3eedd3 100644 --- a/capabilities2_msgs/srv/LaunchStart.srv +++ b/capabilities2_msgs/srv/Launch.srv @@ -2,5 +2,4 @@ std_msgs/Header header string package_name string launch_file_name --- -std_msgs/Header header -int32 pid \ No newline at end of file +std_msgs/Header header \ No newline at end of file diff --git a/capabilities2_msgs/srv/LaunchStop.srv b/capabilities2_msgs/srv/LaunchStop.srv deleted file mode 100644 index 5615899..0000000 --- a/capabilities2_msgs/srv/LaunchStop.srv +++ /dev/null @@ -1,4 +0,0 @@ -std_msgs/Header header -int32 pid ---- -std_msgs/Header header \ No newline at end of file diff --git a/capabilities2_runner/include/capabilities2_runner/launch_runner.hpp b/capabilities2_runner/include/capabilities2_runner/launch_runner.hpp index db53fd3..82b32f4 100644 --- a/capabilities2_runner/include/capabilities2_runner/launch_runner.hpp +++ b/capabilities2_runner/include/capabilities2_runner/launch_runner.hpp @@ -1,8 +1,7 @@ #pragma once #include -#include -#include +#include namespace capabilities2_runner { @@ -15,8 +14,7 @@ namespace capabilities2_runner class LaunchRunner : public RunnerBase { public: - using LaunchStart = capabilities2_msgs::srv::LaunchStart; - using LaunchStop = capabilities2_msgs::srv::LaunchStop; + using Launch = capabilities2_msgs::srv::Launch; /** * @brief Constructor which needs to be empty due to plugin semantics @@ -39,7 +37,7 @@ class LaunchRunner : public RunnerBase launch_name = run_config_.runner.substr(run_config_.runner.find("/") + 1); // create an service client - start_service_client_ = node_->create_client("/capabilities/launch_start"); + start_service_client_ = node_->create_client("/capabilities/launch_start"); RCLCPP_INFO(node_->get_logger(), "%s waiting for service: /capabilities/launch_start", run_config_.interface.c_str()); @@ -55,7 +53,7 @@ class LaunchRunner : public RunnerBase run_config_.interface.c_str()); // create an service client - stop_service_client_ = node_->create_client("/capabilities/launch_stop"); + stop_service_client_ = node_->create_client("/capabilities/launch_stop"); // wait for action server RCLCPP_INFO(node_->get_logger(), "%s waiting for service: /capabilities/launch_stop", @@ -72,7 +70,7 @@ class LaunchRunner : public RunnerBase run_config_.interface.c_str()); // generate a reequest from launch_name and package_name - auto request_msg = std::make_shared(); + auto request_msg = std::make_shared(); request_msg->package_name = package_name; request_msg->launch_file_name = launch_name; @@ -80,7 +78,7 @@ class LaunchRunner : public RunnerBase RCLCPP_INFO(node_->get_logger(), "Requesting to launch %s from %s", launch_name.c_str(), package_name.c_str()); auto result_future = start_service_client_->async_send_request( - request_msg, [this](typename rclcpp::Client::SharedFuture future) { + request_msg, [this](typename rclcpp::Client::SharedFuture future) { if (!future.valid()) { RCLCPP_ERROR(node_->get_logger(), "Request to launch %s from %s failed", launch_name.c_str(), package_name.c_str()); @@ -88,10 +86,6 @@ class LaunchRunner : public RunnerBase } RCLCPP_INFO(node_->get_logger(), "Request to launch %s from %s succeeded", launch_name.c_str(), package_name.c_str()); - - auto response = future.get(); - - childPid = response->pid; }); } @@ -108,14 +102,15 @@ class LaunchRunner : public RunnerBase throw runner_exception("cannot stop runner that was not started"); // generate a reequest from launch_name and package_name - auto request_msg = std::make_shared(); + auto request_msg = std::make_shared(); - request_msg->pid = childPid; + request_msg->package_name = package_name; + request_msg->launch_file_name = launch_name; - RCLCPP_INFO(node_->get_logger(), "Requesting to stop %s with PID %d", launch_name.c_str(), childPid); + RCLCPP_INFO(node_->get_logger(), "Requesting to stop %s from %s", launch_name.c_str(), package_name.c_str()); auto result_future = stop_service_client_->async_send_request( - request_msg, [this](typename rclcpp::Client::SharedFuture future) { + request_msg, [this](typename rclcpp::Client::SharedFuture future) { if (!future.valid()) { RCLCPP_ERROR(node_->get_logger(), "Request to stop %s from %s failed ", launch_name.c_str(), package_name.c_str()); @@ -132,12 +127,11 @@ class LaunchRunner : public RunnerBase throw runner_exception("No Trigger as this is launch runner"); } - int childPid = -1; std::string launch_name; std::string package_name; - rclcpp::Client::SharedPtr start_service_client_; - rclcpp::Client::SharedPtr stop_service_client_; + rclcpp::Client::SharedPtr start_service_client_; + rclcpp::Client::SharedPtr stop_service_client_; }; } // namespace capabilities2_runner \ No newline at end of file diff --git a/capabilities2_server/launch/system.launch.py b/capabilities2_server/launch/system.launch.py new file mode 100644 index 0000000..8b7c0f0 --- /dev/null +++ b/capabilities2_server/launch/system.launch.py @@ -0,0 +1,20 @@ +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from ament_index_python.packages import get_package_share_directory +import os + +def generate_launch_description(): + + navstack_launch_path = os.path.join(get_package_share_directory('nav_stack'), 'launch', 'system.launch.py') + capabilities_launch_path = os.path.join(get_package_share_directory('capabilities2_server'), 'launch', 'server.launch.py') + + return LaunchDescription([ + IncludeLaunchDescription( + PythonLaunchDescriptionSource(navstack_launch_path), + ), + + IncludeLaunchDescription( + PythonLaunchDescriptionSource(capabilities_launch_path), + ) + ]) \ No newline at end of file From dfe616abeecb94e9b9d573c82655f8732140902f Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Fri, 29 Nov 2024 19:06:39 +1100 Subject: [PATCH 056/133] minor fixes --- capabilities2_fabric/docs/waypoint_runner_ex1.md | 2 +- .../include/capabilities2_runner_nav2/waypoint_runner.hpp | 2 +- capabilities2_server/launch/server.launch.py | 1 + 3 files changed, 3 insertions(+), 2 deletions(-) diff --git a/capabilities2_fabric/docs/waypoint_runner_ex1.md b/capabilities2_fabric/docs/waypoint_runner_ex1.md index f2cf7da..5ba7a03 100644 --- a/capabilities2_fabric/docs/waypoint_runner_ex1.md +++ b/capabilities2_fabric/docs/waypoint_runner_ex1.md @@ -46,7 +46,7 @@ ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py ```bash source install/setup.bash -ros2 launch capabilities2_server server.launch.py +ros2 launch capabilities2_server system.launch.py ``` ### Start the fabric 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 337ffaa..fbad70b 100644 --- a/capabilities2_runner_nav2/include/capabilities2_runner_nav2/waypoint_runner.hpp +++ b/capabilities2_runner_nav2/include/capabilities2_runner_nav2/waypoint_runner.hpp @@ -1,7 +1,7 @@ #pragma once #include - +#include #include #include #include diff --git a/capabilities2_server/launch/server.launch.py b/capabilities2_server/launch/server.launch.py index f424544..3f3b098 100644 --- a/capabilities2_server/launch/server.launch.py +++ b/capabilities2_server/launch/server.launch.py @@ -25,6 +25,7 @@ def generate_launch_description(): namespace='', package='rclcpp_components', executable='component_container', + arguments=['--ros-args', '--log-level', 'debug'], composable_node_descriptions=[ ComposableNode( package='capabilities2_server', From e9b1acdc3364e9b24b0ed484b88d4294e53e5cf8 Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Sat, 30 Nov 2024 06:08:31 +1100 Subject: [PATCH 057/133] added gdb and xterm --- capabilities2_fabric/docs/waypoint_runner_ex1.md | 9 ++++++++- capabilities2_server/launch/server.launch.py | 5 +++-- 2 files changed, 11 insertions(+), 3 deletions(-) diff --git a/capabilities2_fabric/docs/waypoint_runner_ex1.md b/capabilities2_fabric/docs/waypoint_runner_ex1.md index 5ba7a03..555c773 100644 --- a/capabilities2_fabric/docs/waypoint_runner_ex1.md +++ b/capabilities2_fabric/docs/waypoint_runner_ex1.md @@ -42,11 +42,18 @@ export TURTLEBOT3_MODEL=waffle ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py ``` +### Start the Navigation2 stack + +```bash +source install/setup.bash +ros2 launch nav_stack system.launch.py +``` + ### Start the Capabilities2 Server ```bash source install/setup.bash -ros2 launch capabilities2_server system.launch.py +ros2 launch capabilities2_server server.launch.py ``` ### Start the fabric diff --git a/capabilities2_server/launch/server.launch.py b/capabilities2_server/launch/server.launch.py index 3f3b098..59f09fe 100644 --- a/capabilities2_server/launch/server.launch.py +++ b/capabilities2_server/launch/server.launch.py @@ -24,8 +24,9 @@ def generate_launch_description(): name='capabilities2_container', namespace='', package='rclcpp_components', - executable='component_container', - arguments=['--ros-args', '--log-level', 'debug'], + executable='component_container_isolated', + prefix=['xterm -e gdb -ex run --args'], # Add GDB debugging prefix + arguments=['--ros-args', '--log-level', 'info'], composable_node_descriptions=[ ComposableNode( package='capabilities2_server', From 6694d25aaa1ef1c6999149aac1c7f2bce95167a2 Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Sun, 1 Dec 2024 21:09:42 +1100 Subject: [PATCH 058/133] Update setup_with_dev.md --- docs/setup_with_dev.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/setup_with_dev.md b/docs/setup_with_dev.md index 145d4ae..05f5297 100644 --- a/docs/setup_with_dev.md +++ b/docs/setup_with_dev.md @@ -17,7 +17,7 @@ Clone the package using Git ```bash git clone -b capabilities2-server-fabric https://github.com/CollaborativeRoboticsLab/capabilities2.git -git clone -b develop https://github.com/CollaborativeRoboticsLab/nav_stack.git +git clone -b develop https://github.com/CollaborativeRoboticsLab/std_capabilities.git git clone https://github.com/AIResearchLab/nav_stack.git ``` From 07c0489b5ad978ce0ede749450802f6f2094d429 Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Sun, 1 Dec 2024 22:22:27 +1100 Subject: [PATCH 059/133] Update setup_with_dev.md --- docs/setup_with_dev.md | 16 +++++++++++++++- 1 file changed, 15 insertions(+), 1 deletion(-) diff --git a/docs/setup_with_dev.md b/docs/setup_with_dev.md index 05f5297..88972ee 100644 --- a/docs/setup_with_dev.md +++ b/docs/setup_with_dev.md @@ -23,7 +23,21 @@ git clone https://github.com/AIResearchLab/nav_stack.git ### Devcontainer -A `devcontainer` is provided for developing the capabilities2 meta-package. The container can be used with [Microsoft Visual Studio Code](https://code.visualstudio.com/) and [Remote Development Extention](https://marketplace.visualstudio.com/items?itemName=ms-vscode-remote.vscode-remote-extensionpack). Dependencies need to be installed in the container. Install the dependencies with rosdep: +A `devcontainer` is provided for developing the capabilities2 meta-package. The container can be used with [Microsoft Visual Studio Code](https://code.visualstudio.com/) and [Remote Development Extention](https://marketplace.visualstudio.com/items?itemName=ms-vscode-remote.vscode-remote-extensionpack). + +If there are multiple packages in the workspace as with the usual case, move the `.devcontainer` folder into the parent folder such that it is located as follows, +```txt +colcon_ws +--> build +--> install +--> log +--> src + --> .devcontainer + --> capabilities2 + --> pakcage .. +``` + +Close and reopen the vs code editor so that devcontainer is automatically detected. After that, Dependencies need to be installed in the container. Install the dependencies with rosdep: ```bash # in the devcontainer From 4df5be5d58213676d0812a47eb5237c7fa380fc9 Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Mon, 2 Dec 2024 23:57:41 +1100 Subject: [PATCH 060/133] minor fixes --- .devcontainer/devcontainer.json | 2 +- capabilities2_fabric/config/fabric.yaml | 2 +- capabilities2_server/launch/server.launch.py | 4 ++-- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json index 9148ff5..e1f8dd5 100644 --- a/.devcontainer/devcontainer.json +++ b/.devcontainer/devcontainer.json @@ -9,7 +9,7 @@ "--privileged", "--network=host" ], - "workspaceMount": "source=${localWorkspaceFolder},target=/home/ubuntu/colcon_ws/src/${localWorkspaceFolderBasename},type=bind", + "workspaceMount": "source=${localWorkspaceFolder},target=/home/ubuntu/colcon_ws/${localWorkspaceFolderBasename},type=bind", "workspaceFolder": "/home/ubuntu/colcon_ws", "mounts": [ "source=${localEnv:HOME}${localEnv:USERPROFILE}/.bash_history,target=/home/ubuntu/.bash_history,type=bind" diff --git a/capabilities2_fabric/config/fabric.yaml b/capabilities2_fabric/config/fabric.yaml index ce64389..59a9a12 100644 --- a/capabilities2_fabric/config/fabric.yaml +++ b/capabilities2_fabric/config/fabric.yaml @@ -1,4 +1,4 @@ -/**: +capabilities2_file_parser: ros__parameters: plan_file_path: "install/capabilities2_fabric/share/capabilities2_fabric/plans/navigation_1.xml" # WaypointRunner Example 1 # plan_file_path: "install/capabilities2_fabric/share/capabilities2_fabric/plans/default.xml" diff --git a/capabilities2_server/launch/server.launch.py b/capabilities2_server/launch/server.launch.py index 59f09fe..a7b4fa8 100644 --- a/capabilities2_server/launch/server.launch.py +++ b/capabilities2_server/launch/server.launch.py @@ -24,8 +24,8 @@ def generate_launch_description(): name='capabilities2_container', namespace='', package='rclcpp_components', - executable='component_container_isolated', - prefix=['xterm -e gdb -ex run --args'], # Add GDB debugging prefix + executable='component_container_mt', + # prefix=['xterm -e gdb -ex run --args'], # Add GDB debugging prefix arguments=['--ros-args', '--log-level', 'info'], composable_node_descriptions=[ ComposableNode( From bf457f3d612a0731e2375c76845fd8a9108fda2a Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Sun, 8 Dec 2024 21:34:59 +1100 Subject: [PATCH 061/133] work in progress --- capabilities2_fabric/config/fabric.yaml | 5 +- .../capabilities_fabric.hpp | 330 ++++++++++-------- capabilities2_fabric/plans/navigation_2.xml | 7 + .../capabilities2_runner/action_runner.hpp | 44 +-- .../capabilities2_runner/runner_base.hpp | 10 +- .../capabilities2_runner/service_runner.hpp | 30 +- .../capabilities2_runner/topic_runner.hpp | 29 +- .../capabilities2_server/capabilities_api.hpp | 2 + .../capabilities_server.hpp | 1 + .../capabilities2_server/runner_cache.hpp | 31 +- capabilities2_server/launch/server.launch.py | 2 +- 11 files changed, 244 insertions(+), 247 deletions(-) create mode 100644 capabilities2_fabric/plans/navigation_2.xml diff --git a/capabilities2_fabric/config/fabric.yaml b/capabilities2_fabric/config/fabric.yaml index 59a9a12..f0f58b6 100644 --- a/capabilities2_fabric/config/fabric.yaml +++ b/capabilities2_fabric/config/fabric.yaml @@ -1,4 +1,5 @@ capabilities2_file_parser: ros__parameters: - plan_file_path: "install/capabilities2_fabric/share/capabilities2_fabric/plans/navigation_1.xml" # WaypointRunner Example 1 - # plan_file_path: "install/capabilities2_fabric/share/capabilities2_fabric/plans/default.xml" + plan_file_path: "install/capabilities2_fabric/share/capabilities2_fabric/plans/navigation_2.xml" # WaypointRunner Example 2 + # plan_file_path: "install/capabilities2_fabric/share/capabilities2_fabric/plans/navigation_1.xml" # WaypointRunner Example 1 + # plan_file_path: "install/capabilities2_fabric/share/capabilities2_fabric/plans/default.xml" diff --git a/capabilities2_fabric/include/capabilities2_executor/capabilities_fabric.hpp b/capabilities2_fabric/include/capabilities2_executor/capabilities_fabric.hpp index be7ca33..f2e8937 100644 --- a/capabilities2_fabric/include/capabilities2_executor/capabilities_fabric.hpp +++ b/capabilities2_fabric/include/capabilities2_executor/capabilities_fabric.hpp @@ -210,6 +210,12 @@ class CapabilitiesFabric : public rclcpp::Node expected_interfaces_ = 0; completed_interfaces_ = 0; + expected_capabilities_ = 0; + completed_capabilities_ = 0; + + expected_configurations_ = 0; + completed_configurations_ = 0; + getInterfaces(goal_handle); } @@ -402,8 +408,8 @@ class CapabilitiesFabric : public rclcpp::Node } else { - feedback->progress = std::to_string(completed_providers_) + "/" + std::to_string(expected_providers_) + " : No providers for " + - requested_interface; + feedback->progress = + std::to_string(completed_providers_) + "/" + std::to_string(expected_providers_) + " : No providers for " + requested_interface; goal_handle->publish_feedback(feedback); RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); } @@ -588,16 +594,9 @@ class CapabilitiesFabric : public rclcpp::Node expected_capabilities_ = connection_map.size(); - // start all runners and interfaces that the connections depend on - for (const auto& [key, value] : connection_map) - { - RCLCPP_INFO(this->get_logger(), ""); - feedback->progress = "Starting capability of Node " + std::to_string(key) + " named " + value.source.runner; - goal_handle->publish_feedback(feedback); - RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); + RCLCPP_INFO(this->get_logger(), "Requsting use of %d capabilities", expected_capabilities_); - use_capability(value.source.runner, value.source.provider, goal_handle); - } + use_capability(connection_map, goal_handle); }); } @@ -608,64 +607,82 @@ class CapabilitiesFabric : public rclcpp::Node * @param provider provider of the capability * @param bond_id bond_id for the capability */ - void use_capability(const std::string& capability, const std::string& provider, const std::shared_ptr goal_handle) + void use_capability(std::map& capabilities, const std::shared_ptr goal_handle) { auto feedback = std::make_shared(); + std::string capability = capabilities[completed_capabilities_].source.runner; + std::string provider = capabilities[completed_capabilities_].source.provider; + auto request_use = std::make_shared(); request_use->capability = capability; request_use->preferred_provider = provider; request_use->bond_id = bond_id; - // send the request - auto result_future = use_capability_client_->async_send_request( - request_use, [this, goal_handle, feedback, capability, provider](UseCapabilityClient::SharedFuture future) { - if (!future.valid()) - { - auto result = std::make_shared(); - result->success = false; - result->message = "Failed to Use capability " + capability + " from " + provider; - goal_handle->abort(result); + feedback->progress = + "Using capability of Node " + std::to_string(completed_capabilities_) + " named " + capabilities[completed_capabilities_].source.runner; + goal_handle->publish_feedback(feedback); + RCLCPP_INFO(this->get_logger(), ""); + RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); - RCLCPP_ERROR(this->get_logger(), result->message.c_str()); - RCLCPP_ERROR(this->get_logger(), "Server Execution Cancelled"); - return; - } + // send the request + auto result_future = use_capability_client_->async_send_request(request_use, [this, goal_handle, feedback, capability, + provider](UseCapabilityClient::SharedFuture future) { + if (!future.valid()) + { + auto result = std::make_shared(); + result->success = false; + result->message = "Failed to Use capability " + capability + " from " + provider; + goal_handle->abort(result); - completed_capabilities_++; + RCLCPP_ERROR(this->get_logger(), result->message.c_str()); + RCLCPP_ERROR(this->get_logger(), "Server Execution Cancelled"); - auto response = future.get(); - feedback->progress = "Successfully started capability " + capability + " from " + provider; + // release all capabilities that were used since not all started successfully + for (const auto& [key, value] : connection_map) + { + feedback->progress = "Freeing capability of Node " + std::to_string(key) + " named " + value.source.runner; goal_handle->publish_feedback(feedback); RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); - // Check if all expected calls are completed before calling verify_plan - if (completed_capabilities_ == expected_capabilities_) - { - RCLCPP_INFO(this->get_logger(), ""); - feedback->progress = "All requested capabilities have started"; - goal_handle->publish_feedback(feedback); - RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); + free_capability(value.source.runner, goal_handle); + } - feedback->progress = "Configuring the capabilities with events"; - goal_handle->publish_feedback(feedback); - RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); + return; + } - configure_capabilities(goal_handle); - } - else - { - // release all capabilities that were used since not all started successfully - for (const auto& [key, value] : connection_map) - { - feedback->progress = "Freeing capability of Node " + std::to_string(key) + " named " + value.source.runner; - goal_handle->publish_feedback(feedback); - RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); + completed_capabilities_++; - free_capability(value.source.runner, goal_handle); - } - } - }); + auto response = future.get(); + + feedback->progress = std::to_string(completed_capabilities_) + "/" + std::to_string(expected_capabilities_) + " : Capability use succeessgul"; + goal_handle->publish_feedback(feedback); + RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); + + // Check if all expected calls are completed before calling verify_plan + if (completed_capabilities_ == expected_capabilities_) + { + RCLCPP_INFO(this->get_logger(), ""); + feedback->progress = "All requested capabilities have been used"; + goal_handle->publish_feedback(feedback); + RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); + + feedback->progress = "Configuring the capabilities with events"; + goal_handle->publish_feedback(feedback); + RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); + + expected_configurations_ = connection_map.size(); + + RCLCPP_INFO(this->get_logger(), ""); + RCLCPP_INFO(this->get_logger(), "Requsting capability configuration for %d capabilities", expected_configurations_); + + configure_capabilities(connection_map, goal_handle); + } + else + { + use_capability(connection_map, goal_handle); + } + }); } /** @@ -705,130 +722,137 @@ class CapabilitiesFabric : public rclcpp::Node /** * @brief Request use of capability from capabilities2 server */ - void configure_capabilities(const std::shared_ptr goal_handle) + void configure_capabilities(std::map& capabilities, const std::shared_ptr goal_handle) { auto feedback = std::make_shared(); + auto request_configure = std::make_shared(); + + RCLCPP_INFO(this->get_logger(), ""); + feedback->progress = "Configuring capability of Node " + std::to_string(completed_configurations_) + " named " + + capabilities[completed_configurations_].source.runner; + goal_handle->publish_feedback(feedback); + RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); - for (const auto& [key, value] : connection_map) + if (capabilities2_xml_parser::convert_to_string(capabilities[completed_configurations_].source.parameters, request_configure->source.parameters)) { - auto request_configure = std::make_shared(); + request_configure->source.capability = capabilities[completed_configurations_].source.runner; + request_configure->source.provider = capabilities[completed_configurations_].source.provider; - RCLCPP_INFO(this->get_logger(), ""); - feedback->progress = "Configuring capability of Node " + std::to_string(key) + " named " + value.source.runner; - goal_handle->publish_feedback(feedback); - RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); + RCLCPP_INFO(this->get_logger(), "Source capability : %s, provider : %s", request_configure->source.capability.c_str(), + request_configure->source.provider.c_str()); + } + else + { + request_configure->source.capability = ""; + request_configure->source.provider = ""; + } - if (capabilities2_xml_parser::convert_to_string(value.source.parameters, request_configure->source.parameters)) - { - request_configure->source.capability = value.source.runner; - request_configure->source.provider = value.source.provider; + if (capabilities2_xml_parser::convert_to_string(capabilities[completed_configurations_].target_on_start.parameters, + request_configure->target_on_start.parameters)) + { + request_configure->target_on_start.capability = capabilities[completed_configurations_].target_on_start.runner; + request_configure->target_on_start.provider = capabilities[completed_configurations_].target_on_start.provider; - RCLCPP_INFO(this->get_logger(), "Source capability : %s, provider : %s", request_configure->source.capability.c_str(), - request_configure->source.provider.c_str()); - } - else - { - request_configure->source.capability = ""; - request_configure->source.provider = ""; - } + RCLCPP_INFO(this->get_logger(), "--> on_start capability : %s provider : %s", request_configure->target_on_start.capability.c_str(), + request_configure->target_on_start.provider.c_str()); + } + else + { + request_configure->target_on_start.capability = ""; + request_configure->target_on_start.provider = ""; + } - if (capabilities2_xml_parser::convert_to_string(value.target_on_start.parameters, request_configure->target_on_start.parameters)) - { - request_configure->target_on_start.capability = value.target_on_start.runner; - request_configure->target_on_start.provider = value.target_on_start.provider; + if (capabilities2_xml_parser::convert_to_string(capabilities[completed_configurations_].target_on_stop.parameters, + request_configure->target_on_stop.parameters)) + { + request_configure->target_on_stop.capability = capabilities[completed_configurations_].target_on_stop.runner; + request_configure->target_on_stop.provider = capabilities[completed_configurations_].target_on_stop.provider; - RCLCPP_INFO(this->get_logger(), "Triggered on start capability : %s provider : %s", request_configure->target_on_start.capability.c_str(), - request_configure->target_on_start.provider.c_str()); - } - else - { - request_configure->target_on_start.capability = ""; - request_configure->target_on_start.provider = ""; - } + RCLCPP_INFO(this->get_logger(), "--> on stop capability : %s provider : %s", request_configure->target_on_stop.capability.c_str(), + request_configure->target_on_stop.provider.c_str()); + } + else + { + request_configure->target_on_stop.capability = ""; + request_configure->target_on_stop.provider = ""; + } - if (capabilities2_xml_parser::convert_to_string(value.target_on_stop.parameters, request_configure->target_on_stop.parameters)) - { - request_configure->target_on_stop.capability = value.target_on_stop.runner; - request_configure->target_on_stop.provider = value.target_on_stop.provider; + if (capabilities2_xml_parser::convert_to_string(capabilities[completed_configurations_].target_on_success.parameters, + request_configure->target_on_success.parameters)) + { + request_configure->target_on_success.capability = capabilities[completed_configurations_].target_on_success.runner; + request_configure->target_on_success.provider = capabilities[completed_configurations_].target_on_success.provider; - RCLCPP_INFO(this->get_logger(), "Triggered on stop capability : %s provider : %s", request_configure->target_on_stop.capability.c_str(), - request_configure->target_on_stop.provider.c_str()); - } - else - { - request_configure->target_on_stop.capability = ""; - request_configure->target_on_stop.provider = ""; - } + RCLCPP_INFO(this->get_logger(), "--> on success capability : %s provider : %s", request_configure->target_on_success.capability.c_str(), + request_configure->target_on_success.provider.c_str()); + } + else + { + request_configure->target_on_success.capability = ""; + request_configure->target_on_success.provider = ""; + } - if (capabilities2_xml_parser::convert_to_string(value.target_on_success.parameters, request_configure->target_on_success.parameters)) - { - request_configure->target_on_success.capability = value.target_on_success.runner; - request_configure->target_on_success.provider = value.target_on_success.provider; + if (capabilities2_xml_parser::convert_to_string(capabilities[completed_configurations_].target_on_failure.parameters, + request_configure->target_on_failure.parameters)) + { + request_configure->target_on_failure.capability = capabilities[completed_configurations_].target_on_failure.runner; + request_configure->target_on_failure.provider = capabilities[completed_configurations_].target_on_failure.provider; - RCLCPP_INFO(this->get_logger(), "Triggered on success capability : %s provider : %s", request_configure->target_on_success.capability.c_str(), - request_configure->target_on_success.provider.c_str()); - } - else - { - request_configure->target_on_success.capability = ""; - request_configure->target_on_success.provider = ""; - } + RCLCPP_INFO(this->get_logger(), "--> on failure capability : %s provider : %s", request_configure->target_on_failure.capability.c_str(), + request_configure->target_on_failure.provider.c_str()); + } + else + { + request_configure->target_on_failure.capability = ""; + request_configure->target_on_failure.provider = ""; + } - if (capabilities2_xml_parser::convert_to_string(value.target_on_failure.parameters, request_configure->target_on_failure.parameters)) - { - request_configure->target_on_failure.capability = value.target_on_failure.runner; - request_configure->target_on_failure.provider = value.target_on_failure.provider; + std::string source_capability = capabilities[completed_configurations_].source.runner; - RCLCPP_INFO(this->get_logger(), "Triggered on failure capability : %s provider : %s", request_configure->target_on_failure.capability.c_str(), - request_configure->target_on_failure.provider.c_str()); - } - else - { - request_configure->target_on_failure.capability = ""; - request_configure->target_on_failure.provider = ""; - } + // send the request + auto result_future = configure_capability_client_->async_send_request( + request_configure, [this, goal_handle, source_capability, feedback](ConfigureCapabilityClient::SharedFuture future) { + if (!future.valid()) + { + auto result = std::make_shared(); + result->success = false; + result->message = "Failed to configure capability :" + source_capability; + goal_handle->abort(result); + + RCLCPP_ERROR(this->get_logger(), result->message.c_str()); + RCLCPP_ERROR(this->get_logger(), "Server Execution Cancelled"); + return; + } - expected_configurations_++; + completed_configurations_++; - // send the request - auto result_future = configure_capability_client_->async_send_request( - request_configure, [this, goal_handle, value, feedback](ConfigureCapabilityClient::SharedFuture future) { - if (!future.valid()) - { - auto result = std::make_shared(); - result->success = false; - result->message = "Failed to configure capability :" + value.source.runner; - goal_handle->abort(result); - - RCLCPP_ERROR(this->get_logger(), result->message.c_str()); - RCLCPP_ERROR(this->get_logger(), "Server Execution Cancelled"); - return; - } + auto response = future.get(); - completed_configurations_++; + feedback->progress = std::to_string(completed_configurations_) + "/" + std::to_string(expected_configurations_) + + " : Successfully configured capability : " + source_capability; + goal_handle->publish_feedback(feedback); + RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); - auto response = future.get(); - feedback->progress = "Successfully configured capability :" + value.source.runner; + // Check if all expected calls are completed before calling verify_plan + if (completed_configurations_ == expected_configurations_) + { + RCLCPP_INFO(this->get_logger(), ""); + feedback->progress = "All requested capabilities have been configured"; goal_handle->publish_feedback(feedback); RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); - // Check if all expected calls are completed before calling verify_plan - if (completed_configurations_ == expected_configurations_) - { - RCLCPP_INFO(this->get_logger(), ""); - feedback->progress = "All requested capabilities have been configured"; - goal_handle->publish_feedback(feedback); - RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); - - RCLCPP_INFO(this->get_logger(), ""); - feedback->progress = "Triggering the first capability"; - goal_handle->publish_feedback(feedback); - RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); + RCLCPP_INFO(this->get_logger(), ""); + feedback->progress = "Triggering the first capability"; + goal_handle->publish_feedback(feedback); + RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); - trigger_first_node(goal_handle); - } - }); - } + trigger_first_node(goal_handle); + } + else + { + configure_capabilities(connection_map, goal_handle); + } + }); } /** diff --git a/capabilities2_fabric/plans/navigation_2.xml b/capabilities2_fabric/plans/navigation_2.xml new file mode 100644 index 0000000..e87805d --- /dev/null +++ b/capabilities2_fabric/plans/navigation_2.xml @@ -0,0 +1,7 @@ + + + + + + + \ No newline at end of file diff --git a/capabilities2_runner/include/capabilities2_runner/action_runner.hpp b/capabilities2_runner/include/capabilities2_runner/action_runner.hpp index e3d21ed..f438e52 100644 --- a/capabilities2_runner/include/capabilities2_runner/action_runner.hpp +++ b/capabilities2_runner/include/capabilities2_runner/action_runner.hpp @@ -92,11 +92,7 @@ class ActionRunner : public RunnerBase } // Trigger on_stopped event if defined - if (events[execute_id].on_stopped) - { - events[execute_id].on_stopped(update_on_stopped(events[execute_id].on_stopped_param)); - execute_id += 1; - } + events[execute_id].on_stopped(update_on_stopped(events[execute_id].on_stopped_param)); }); // wait for action to be stopped. hold the thread for 2 seconds to help keep callbacks in scope @@ -132,6 +128,8 @@ class ActionRunner : public RunnerBase virtual std::optional> trigger(tinyxml2::XMLElement* parameters = nullptr) override { + execute_id += 1; + // if parameters are not provided then cannot proceed if (!parameters) throw runner_exception("cannot trigger action without parameters"); @@ -144,18 +142,12 @@ class ActionRunner : public RunnerBase send_goal_options_.goal_response_callback = [this](const typename rclcpp_action::ClientGoalHandle::SharedPtr& goal_handle) { if (goal_handle) - { - // Trigger on_started event if defined - if (events[execute_id].on_started) - { - events[execute_id].on_started(update_on_started(events[execute_id].on_started_param)); - execute_id += 1; - } - } + events[execute_id].on_started(update_on_started(events[execute_id].on_started_param)); else { RCLCPP_ERROR(node_->get_logger(), "Goal was rejected by server"); } + // store goal handle to be used with stop funtion goal_handle_ = goal_handle; }; @@ -163,26 +155,12 @@ class ActionRunner : public RunnerBase // result callback send_goal_options_.result_callback = [this](const typename rclcpp_action::ClientGoalHandle::WrappedResult& wrapped_result) { - if (wrapped_result.code == rclcpp_action::ResultCode::SUCCEEDED) - { - // Trigger on_success event if defined - if (events[execute_id].on_success) - { - result_ = wrapped_result.result; - events[execute_id].on_success(update_on_success(events[execute_id].on_success_param)); - execute_id += 1; - } - } - else - { - // Trigger on_failure event if defined - if (events[execute_id].on_failure) - { - result_ = wrapped_result.result; - events[execute_id].on_failure(update_on_failure(events[execute_id].on_failure_param)); - execute_id += 1; - } - } + if (wrapped_result.code == rclcpp_action::ResultCode::SUCCEEDED) + events[execute_id].on_success(update_on_success(events[execute_id].on_success_param)); // Trigger on_success event if defined + else + events[execute_id].on_failure(update_on_failure(events[execute_id].on_failure_param)); // Trigger on_failure event if defined + + result_ = wrapped_result.result; }; // trigger the action client with goal diff --git a/capabilities2_runner/include/capabilities2_runner/runner_base.hpp b/capabilities2_runner/include/capabilities2_runner/runner_base.hpp index 42913f1..4027760 100644 --- a/capabilities2_runner/include/capabilities2_runner/runner_base.hpp +++ b/capabilities2_runner/include/capabilities2_runner/runner_base.hpp @@ -137,18 +137,24 @@ class RunnerBase node_ = node; run_config_ = run_config; insert_id = 0; - execute_id = 0; + execute_id = -1; } /** * @brief attach events to the runner * * @param event_option event_options related for the action + * + * @return number of attached events */ - void attach_events(capabilities2_runner::event_opts& event_option) + int attach_events(capabilities2_runner::event_opts& event_option) { + RCLCPP_INFO(node_->get_logger(), "%s accepted event options with ID : %d ", run_config_.interface.c_str(), insert_id); + events[insert_id] = event_option; insert_id += 1; + + return insert_id; } /** diff --git a/capabilities2_runner/include/capabilities2_runner/service_runner.hpp b/capabilities2_runner/include/capabilities2_runner/service_runner.hpp index 10e205f..73201d8 100644 --- a/capabilities2_runner/include/capabilities2_runner/service_runner.hpp +++ b/capabilities2_runner/include/capabilities2_runner/service_runner.hpp @@ -62,6 +62,8 @@ class ServiceRunner : public RunnerBase virtual std::optional> trigger(tinyxml2::XMLElement* parameters = nullptr) override { + execute_id += 1; + // if parameters are not provided then cannot proceed if (!parameters) throw runner_exception("cannot trigger service without parameters"); @@ -74,33 +76,17 @@ class ServiceRunner : public RunnerBase if (!future.valid()) { RCLCPP_ERROR(node_->get_logger(), "get result call failed"); - - // Trigger failure event - if (events[execute_id].on_failure) - { - events[execute_id].on_failure(update_on_failure(events[execute_id].on_failure_param)); - execute_id += 1; - } + events[execute_id].on_failure(update_on_failure(events[execute_id].on_failure_param)); } else { RCLCPP_INFO(node_->get_logger(), "get result call succeeded"); - - // Trigger success event - if (events[execute_id].on_success) - { - events[execute_id].on_success(update_on_success(events[execute_id].on_success_param)); - execute_id += 1; - } + events[execute_id].on_success(update_on_success(events[execute_id].on_success_param)); } }); // Trigger started event if defined - if (events[execute_id].on_started) - { - events[execute_id].on_started(update_on_started(events[execute_id].on_started_param)); - execute_id += 1; - } + events[execute_id].on_started(update_on_started(events[execute_id].on_started_param)); // Define a callback function to handle the result once it's ready std::function result_callback = @@ -142,11 +128,7 @@ class ServiceRunner : public RunnerBase throw runner_exception("cannot stop runner action that was not started"); // publish event - if (events[execute_id].on_stopped) - { - events[execute_id].on_stopped(update_on_stopped(events[execute_id].on_stopped_param)); - execute_id += 1; - } + events[execute_id].on_stopped(update_on_stopped(events[execute_id].on_stopped_param)); } protected: diff --git a/capabilities2_runner/include/capabilities2_runner/topic_runner.hpp b/capabilities2_runner/include/capabilities2_runner/topic_runner.hpp index 5b45b3e..266d22c 100644 --- a/capabilities2_runner/include/capabilities2_runner/topic_runner.hpp +++ b/capabilities2_runner/include/capabilities2_runner/topic_runner.hpp @@ -52,35 +52,22 @@ class TopicRunner : public RunnerBase virtual std::optional> trigger(tinyxml2::XMLElement* parameters = nullptr) override { + execute_id += 1; + // if parameters are not provided then cannot proceed if (!parameters) throw runner_exception("cannot grab data without parameters"); - if (events[execute_id].on_started) - { - events[execute_id].on_started(update_on_started(events[execute_id].on_started_param)); - execute_id += 1; - } + events[execute_id].on_started(update_on_started(events[execute_id].on_started_param)); if (latest_message_) { - // send success event - if (events[execute_id].on_success) - { - events[execute_id].on_success(update_on_success(events[execute_id].on_success_param)); - execute_id += 1; - } + events[execute_id].on_success(update_on_success(events[execute_id].on_success_param)); } else { RCLCPP_ERROR(node_->get_logger(), "get result call failed"); - - // send terminated event - if (events[execute_id].on_failure) - { - events[execute_id].on_failure(update_on_failure(events[execute_id].on_failure_param)); - execute_id += 1; - } + events[execute_id].on_failure(update_on_failure(events[execute_id].on_failure_param)); } // create a function to call for the result. the future will be returned to the caller and the caller @@ -113,11 +100,7 @@ class TopicRunner : public RunnerBase throw runner_exception("cannot stop runner subscriber that was not started"); // publish event - if (events[execute_id].on_stopped) - { - events[execute_id].on_stopped(update_on_stopped(events[execute_id].on_stopped_param)); - execute_id += 1; - } + events[execute_id].on_stopped(update_on_stopped(events[execute_id].on_stopped_param)); } protected: diff --git a/capabilities2_server/include/capabilities2_server/capabilities_api.hpp b/capabilities2_server/include/capabilities2_server/capabilities_api.hpp index 04c6c88..95575ae 100644 --- a/capabilities2_server/include/capabilities2_server/capabilities_api.hpp +++ b/capabilities2_server/include/capabilities2_server/capabilities_api.hpp @@ -52,6 +52,8 @@ class CapabilitiesAPI // set logger node_logging_interface_ptr_ = node_logging_interface_ptr; + runner_cache_.connect(node_logging_interface_ptr); + // connect db cap_db_ = std::make_unique(db_file); diff --git a/capabilities2_server/include/capabilities2_server/capabilities_server.hpp b/capabilities2_server/include/capabilities2_server/capabilities_server.hpp index f62b5f1..b5c5edd 100644 --- a/capabilities2_server/include/capabilities2_server/capabilities_server.hpp +++ b/capabilities2_server/include/capabilities2_server/capabilities_server.hpp @@ -102,6 +102,7 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI // init capabilities api connect(db_file, get_node_logging_interface()); + // load capabilities from package paths for (const auto& package_path : package_paths) diff --git a/capabilities2_server/include/capabilities2_server/runner_cache.hpp b/capabilities2_server/include/capabilities2_server/runner_cache.hpp index c66defa..9c6ae32 100644 --- a/capabilities2_server/include/capabilities2_server/runner_cache.hpp +++ b/capabilities2_server/include/capabilities2_server/runner_cache.hpp @@ -138,9 +138,9 @@ class RunnerCache if (on_started_capability != "") { event_options.on_started = [this, &on_started_capability, &capability](tinyxml2::XMLElement* parameters) { - runner_cache_[on_started_capability]->trigger(parameters); RCLCPP_INFO(node_logging_interface_ptr_->get_logger(), "%s is triggering %s on_start", capability.c_str(), on_started_capability.c_str()); + runner_cache_[on_started_capability]->trigger(parameters); }; tinyxml2::XMLDocument doc; @@ -149,16 +149,18 @@ class RunnerCache } else { - event_options.on_started = nullptr; - event_options.on_started_param = nullptr; + event_options.on_started = [this, &capability](tinyxml2::XMLElement* parameters) { + RCLCPP_INFO(node_logging_interface_ptr_->get_logger(), "%s does not implement on_start event", + capability.c_str()); + } event_options.on_started_param = nullptr; } if (on_failure_capability != "") { event_options.on_failure = [this, &on_failure_capability, &capability](tinyxml2::XMLElement* parameters) { - runner_cache_[on_failure_capability]->trigger(parameters); RCLCPP_INFO(node_logging_interface_ptr_->get_logger(), "%s is triggering %s on_failure", capability.c_str(), on_failure_capability.c_str()); + runner_cache_[on_failure_capability]->trigger(parameters); }; tinyxml2::XMLDocument doc; @@ -167,7 +169,10 @@ class RunnerCache } else { - event_options.on_failure = nullptr; + event_options.on_failure = [this, &capability](tinyxml2::XMLElement* parameters) { + RCLCPP_INFO(node_logging_interface_ptr_->get_logger(), "%s does not implement on_failure event", + capability.c_str()); + } event_options.on_failure_param = nullptr; } @@ -185,7 +190,10 @@ class RunnerCache } else { - event_options.on_success = nullptr; + event_options.on_success = [this, &capability](tinyxml2::XMLElement* parameters) { + RCLCPP_INFO(node_logging_interface_ptr_->get_logger(), "%s does not implement on_success event", + capability.c_str()); + } event_options.on_success_param = nullptr; } @@ -203,12 +211,17 @@ class RunnerCache } else { - event_options.on_stopped = nullptr; + event_options.on_stopped = [this, &capability](tinyxml2::XMLElement* parameters) { + RCLCPP_INFO(node_logging_interface_ptr_->get_logger(), "%s does not implement on_stopped event", + capability.c_str()); + } event_options.on_stopped_param = nullptr; } - runner_cache_[capability]->attach_events(event_options); - event_cache_[capability] = event_options; + int event_count = runner_cache_[capability]->attach_events(event_options); + + std::string event_name = capability + "_" + std::to_string(event_count); + event_cache_[event_name] = event_options; } /** diff --git a/capabilities2_server/launch/server.launch.py b/capabilities2_server/launch/server.launch.py index a7b4fa8..4f70a3e 100644 --- a/capabilities2_server/launch/server.launch.py +++ b/capabilities2_server/launch/server.launch.py @@ -24,7 +24,7 @@ def generate_launch_description(): name='capabilities2_container', namespace='', package='rclcpp_components', - executable='component_container_mt', + executable='component_container', # prefix=['xterm -e gdb -ex run --args'], # Add GDB debugging prefix arguments=['--ros-args', '--log-level', 'info'], composable_node_descriptions=[ From f28eb69703a81460d7bd7b84ec6b2d71d7fb7498 Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Wed, 1 Jan 2025 17:48:30 +1100 Subject: [PATCH 062/133] work-in-progress --- .../docs/waypoint_runner_ex2.md | 64 +++++++++++++++++++ .../capabilities_fabric.hpp | 19 +++--- capabilities2_fabric/readme.md | 6 +- .../capabilities2_server/capabilities_api.hpp | 11 ++-- .../capabilities2_server/runner_cache.hpp | 13 ++-- 5 files changed, 95 insertions(+), 18 deletions(-) create mode 100644 capabilities2_fabric/docs/waypoint_runner_ex2.md diff --git a/capabilities2_fabric/docs/waypoint_runner_ex2.md b/capabilities2_fabric/docs/waypoint_runner_ex2.md new file mode 100644 index 0000000..01121e9 --- /dev/null +++ b/capabilities2_fabric/docs/waypoint_runner_ex2.md @@ -0,0 +1,64 @@ +## WaypointRunner Example 2 + +### Dependencies + +This example uses nav2 stack and turtlebot3. + +Run the following commands to install nav2 stack + +```bash +sudo apt install ros-humble-navigation2 ros-humble-nav2-bringup +``` + +Run the following commands to install turtlebot3 + +```bash +sudo apt install ros-humble-turtlebot3* +``` + +Clone the nav_stack default configuration and launch files to the same workspace if its not already availabe in the workspace. Capabilities2 Nav2 Runners are dependent on this package. + +```bash +cd src +git clone https://github.com/CollaborativeRoboticsLab/nav_stack.git +``` + +### Plan selection + +Uncomment the line related to `navigation_2.xml` in the `config/fabric,yaml` file + +### Build the package to apply changes + +In the workspace root run, + +```bash +colcon build +``` + +### Start the turtlebot simulation + +```bash +export TURTLEBOT3_MODEL=waffle +ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py +``` + +### Start the Navigation2 stack + +```bash +source install/setup.bash +ros2 launch nav_stack system.launch.py +``` + +### Start the Capabilities2 Server + +```bash +source install/setup.bash +ros2 launch capabilities2_server server.launch.py +``` + +### Start the fabric + +```bash +source install/setup.bash +ros2 launch capabilities2_fabric fabric.launch.py +``` \ No newline at end of file diff --git a/capabilities2_fabric/include/capabilities2_executor/capabilities_fabric.hpp b/capabilities2_fabric/include/capabilities2_executor/capabilities_fabric.hpp index f2e8937..ef5bb30 100644 --- a/capabilities2_fabric/include/capabilities2_executor/capabilities_fabric.hpp +++ b/capabilities2_fabric/include/capabilities2_executor/capabilities_fabric.hpp @@ -588,13 +588,13 @@ class CapabilitiesFabric : public rclcpp::Node auto response = future.get(); bond_id = response->bond_id; - feedback->progress = "Received the bond id"; + feedback->progress = "Received the bond id : " + bond_id.c_str() ; goal_handle->publish_feedback(feedback); RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); expected_capabilities_ = connection_map.size(); - RCLCPP_INFO(this->get_logger(), "Requsting use of %d capabilities", expected_capabilities_); + RCLCPP_INFO(this->get_logger(), "Requsting use (start) of %d capabilities", expected_capabilities_); use_capability(connection_map, goal_handle); }); @@ -620,7 +620,7 @@ class CapabilitiesFabric : public rclcpp::Node request_use->bond_id = bond_id; feedback->progress = - "Using capability of Node " + std::to_string(completed_capabilities_) + " named " + capabilities[completed_capabilities_].source.runner; + "Using (starting) capability of Node " + std::to_string(completed_capabilities_) + " named " + capabilities[completed_capabilities_].source.runner; goal_handle->publish_feedback(feedback); RCLCPP_INFO(this->get_logger(), ""); RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); @@ -655,7 +655,7 @@ class CapabilitiesFabric : public rclcpp::Node auto response = future.get(); - feedback->progress = std::to_string(completed_capabilities_) + "/" + std::to_string(expected_capabilities_) + " : Capability use succeessgul"; + feedback->progress = std::to_string(completed_capabilities_) + "/" + std::to_string(expected_capabilities_) + " : Capability use (start) succeessful"; goal_handle->publish_feedback(feedback); RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); @@ -738,7 +738,7 @@ class CapabilitiesFabric : public rclcpp::Node request_configure->source.capability = capabilities[completed_configurations_].source.runner; request_configure->source.provider = capabilities[completed_configurations_].source.provider; - RCLCPP_INFO(this->get_logger(), "Source capability : %s, provider : %s", request_configure->source.capability.c_str(), + RCLCPP_INFO(this->get_logger(), "Source capability : %s from provider : %s", request_configure->source.capability.c_str(), request_configure->source.provider.c_str()); } else @@ -753,7 +753,7 @@ class CapabilitiesFabric : public rclcpp::Node request_configure->target_on_start.capability = capabilities[completed_configurations_].target_on_start.runner; request_configure->target_on_start.provider = capabilities[completed_configurations_].target_on_start.provider; - RCLCPP_INFO(this->get_logger(), "--> on_start capability : %s provider : %s", request_configure->target_on_start.capability.c_str(), + RCLCPP_INFO(this->get_logger(), "--> on_start capability : %s from provider : %s", request_configure->target_on_start.capability.c_str(), request_configure->target_on_start.provider.c_str()); } else @@ -768,7 +768,7 @@ class CapabilitiesFabric : public rclcpp::Node request_configure->target_on_stop.capability = capabilities[completed_configurations_].target_on_stop.runner; request_configure->target_on_stop.provider = capabilities[completed_configurations_].target_on_stop.provider; - RCLCPP_INFO(this->get_logger(), "--> on stop capability : %s provider : %s", request_configure->target_on_stop.capability.c_str(), + RCLCPP_INFO(this->get_logger(), "--> on stop capability : %s from provider : %s", request_configure->target_on_stop.capability.c_str(), request_configure->target_on_stop.provider.c_str()); } else @@ -783,7 +783,7 @@ class CapabilitiesFabric : public rclcpp::Node request_configure->target_on_success.capability = capabilities[completed_configurations_].target_on_success.runner; request_configure->target_on_success.provider = capabilities[completed_configurations_].target_on_success.provider; - RCLCPP_INFO(this->get_logger(), "--> on success capability : %s provider : %s", request_configure->target_on_success.capability.c_str(), + RCLCPP_INFO(this->get_logger(), "--> on success capability : %s from provider : %s", request_configure->target_on_success.capability.c_str(), request_configure->target_on_success.provider.c_str()); } else @@ -798,7 +798,7 @@ class CapabilitiesFabric : public rclcpp::Node request_configure->target_on_failure.capability = capabilities[completed_configurations_].target_on_failure.runner; request_configure->target_on_failure.provider = capabilities[completed_configurations_].target_on_failure.provider; - RCLCPP_INFO(this->get_logger(), "--> on failure capability : %s provider : %s", request_configure->target_on_failure.capability.c_str(), + RCLCPP_INFO(this->get_logger(), "--> on failure capability : %s from provider : %s", request_configure->target_on_failure.capability.c_str(), request_configure->target_on_failure.provider.c_str()); } else @@ -831,6 +831,7 @@ class CapabilitiesFabric : public rclcpp::Node feedback->progress = std::to_string(completed_configurations_) + "/" + std::to_string(expected_configurations_) + " : Successfully configured capability : " + source_capability; goal_handle->publish_feedback(feedback); + RCLCPP_INFO(this->get_logger(), ""); RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); // Check if all expected calls are completed before calling verify_plan diff --git a/capabilities2_fabric/readme.md b/capabilities2_fabric/readme.md index 96e449e..f541a5a 100644 --- a/capabilities2_fabric/readme.md +++ b/capabilities2_fabric/readme.md @@ -72,4 +72,8 @@ Below is an example XML plan for configuring a set of capabilities: ### Navigation -1. [WaypointRunner Example 1](./docs/waypoint_runner_ex1.md) \ No newline at end of file +1. [WaypointRunner Example 1](./docs/waypoint_runner_ex1.md) +Implements at the very basic fabric triggering that moves the robot from one point to another + +2. [WaypointRunner Example 2](./docs/waypoint_runner_ex2.md) +Implements navigating through 2 points using 'sequential' control functionality \ No newline at end of file diff --git a/capabilities2_server/include/capabilities2_server/capabilities_api.hpp b/capabilities2_server/include/capabilities2_server/capabilities_api.hpp index 95575ae..45f2bc0 100644 --- a/capabilities2_server/include/capabilities2_server/capabilities_api.hpp +++ b/capabilities2_server/include/capabilities2_server/capabilities_api.hpp @@ -84,7 +84,7 @@ class CapabilitiesAPI * @param node ros node pointer of the ros server * @param capability capability name to be started * @param provider provider of the capability - * + * * @return `true` if capability started successfully. else returns `false` */ bool start_capability(rclcpp::Node::SharedPtr node, const std::string& capability, const std::string& provider) @@ -238,7 +238,7 @@ class CapabilitiesAPI * @param capability capability name to be started * @param provider provider of the capability * @param bond_id bond_id for the capability - * + * * @return `true` if capability started successfully. else returns `false` */ bool use_capability(rclcpp::Node::SharedPtr node, const std::string& capability, const std::string& provider, @@ -272,12 +272,15 @@ class CapabilitiesAPI { try { + // log + RCLCPP_INFO(node_logging_interface_ptr_->get_logger(), ""); + RCLCPP_INFO(node_logging_interface_ptr_->get_logger(), "setting triggers for capability: %s", capability.c_str()); + runner_cache_.set_runner_triggers(capability, on_started_capability, on_started_parameters, on_failure_capability, on_failure_parameters, on_success_capability, on_success_parameters, on_stopped_capability, on_stopped_parameters); - // log - RCLCPP_INFO(node_logging_interface_ptr_->get_logger(), "setting triggers for capability: %s", capability.c_str()); + RCLCPP_WARN(node_logging_interface_ptr_->get_logger(), "successfully set triggers"); } catch (const capabilities2_runner::runner_exception& e) { diff --git a/capabilities2_server/include/capabilities2_server/runner_cache.hpp b/capabilities2_server/include/capabilities2_server/runner_cache.hpp index 9c6ae32..1ebd874 100644 --- a/capabilities2_server/include/capabilities2_server/runner_cache.hpp +++ b/capabilities2_server/include/capabilities2_server/runner_cache.hpp @@ -152,7 +152,9 @@ class RunnerCache event_options.on_started = [this, &capability](tinyxml2::XMLElement* parameters) { RCLCPP_INFO(node_logging_interface_ptr_->get_logger(), "%s does not implement on_start event", capability.c_str()); - } event_options.on_started_param = nullptr; + }; + + event_options.on_started_param = nullptr; } if (on_failure_capability != "") @@ -172,7 +174,8 @@ class RunnerCache event_options.on_failure = [this, &capability](tinyxml2::XMLElement* parameters) { RCLCPP_INFO(node_logging_interface_ptr_->get_logger(), "%s does not implement on_failure event", capability.c_str()); - } + } ; + event_options.on_failure_param = nullptr; } @@ -193,7 +196,8 @@ class RunnerCache event_options.on_success = [this, &capability](tinyxml2::XMLElement* parameters) { RCLCPP_INFO(node_logging_interface_ptr_->get_logger(), "%s does not implement on_success event", capability.c_str()); - } + } ; + event_options.on_success_param = nullptr; } @@ -214,7 +218,8 @@ class RunnerCache event_options.on_stopped = [this, &capability](tinyxml2::XMLElement* parameters) { RCLCPP_INFO(node_logging_interface_ptr_->get_logger(), "%s does not implement on_stopped event", capability.c_str()); - } + } ; + event_options.on_stopped_param = nullptr; } From d02a4abd5489ee9470cf80b2351b9030926c66d2 Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Tue, 7 Jan 2025 14:15:14 +1100 Subject: [PATCH 063/133] updated codebase for review. still in progress --- .../docs/waypoint_runner_ex1.md | 2 +- .../docs/waypoint_runner_ex2.md | 2 +- .../capabilities_fabric.hpp | 8 +- .../capabilities_xml_parser.hpp | 6 +- capabilities2_fabric/plans/navigation_1.xml | 2 +- capabilities2_fabric/plans/navigation_2.xml | 4 +- .../capabilities2_runner/action_runner.hpp | 108 +++++------ .../capabilities2_runner/launch_runner.hpp | 9 +- .../notrigger_action_runner.hpp | 15 +- .../capabilities2_runner/runner_base.hpp | 146 +++++++++++---- .../capabilities2_runner/service_runner.hpp | 78 ++++---- .../capabilities2_runner/topic_runner.hpp | 61 +++--- capabilities2_runner/src/standard_runners.cpp | 11 +- .../listen_runner.hpp | 33 +--- .../speak_runner.hpp | 13 -- .../bt_factory_runner.hpp | 38 ++-- .../occupancygrid_runner.hpp | 174 ++++-------------- .../robotpose_runner.hpp | 88 ++------- .../waypoint_runner.hpp | 14 +- .../prompt_occupancy_runner.hpp | 17 -- .../prompt_plan_request_runner.hpp | 46 ++--- .../prompt_plan_response_runner.hpp | 26 +-- .../prompt_pose_runner.hpp | 17 -- .../prompt_text_runner.hpp | 17 -- .../capabilities2_server/capabilities_api.hpp | 2 +- .../capabilities2_server/runner_cache.hpp | 122 +++--------- capabilities2_server/launch/server.launch.py | 2 +- 27 files changed, 388 insertions(+), 673 deletions(-) diff --git a/capabilities2_fabric/docs/waypoint_runner_ex1.md b/capabilities2_fabric/docs/waypoint_runner_ex1.md index 555c773..76ef73a 100644 --- a/capabilities2_fabric/docs/waypoint_runner_ex1.md +++ b/capabilities2_fabric/docs/waypoint_runner_ex1.md @@ -39,7 +39,7 @@ colcon build ```bash export TURTLEBOT3_MODEL=waffle -ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py +ros2 launch turtlebot3_gazebo empty_world.launch.py ``` ### Start the Navigation2 stack diff --git a/capabilities2_fabric/docs/waypoint_runner_ex2.md b/capabilities2_fabric/docs/waypoint_runner_ex2.md index 01121e9..6cf251a 100644 --- a/capabilities2_fabric/docs/waypoint_runner_ex2.md +++ b/capabilities2_fabric/docs/waypoint_runner_ex2.md @@ -39,7 +39,7 @@ colcon build ```bash export TURTLEBOT3_MODEL=waffle -ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py +ros2 launch turtlebot3_gazebo turtlebot3_empty_world.launch.py ``` ### Start the Navigation2 stack diff --git a/capabilities2_fabric/include/capabilities2_executor/capabilities_fabric.hpp b/capabilities2_fabric/include/capabilities2_executor/capabilities_fabric.hpp index ef5bb30..a1396a7 100644 --- a/capabilities2_fabric/include/capabilities2_executor/capabilities_fabric.hpp +++ b/capabilities2_fabric/include/capabilities2_executor/capabilities_fabric.hpp @@ -588,13 +588,13 @@ class CapabilitiesFabric : public rclcpp::Node auto response = future.get(); bond_id = response->bond_id; - feedback->progress = "Received the bond id : " + bond_id.c_str() ; + feedback->progress = "Received the bond id : " + bond_id ; goal_handle->publish_feedback(feedback); RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); expected_capabilities_ = connection_map.size(); - RCLCPP_INFO(this->get_logger(), "Requsting use (start) of %d capabilities", expected_capabilities_); + RCLCPP_INFO(this->get_logger(), "Requsting start of %d capabilities", expected_capabilities_); use_capability(connection_map, goal_handle); }); @@ -620,7 +620,7 @@ class CapabilitiesFabric : public rclcpp::Node request_use->bond_id = bond_id; feedback->progress = - "Using (starting) capability of Node " + std::to_string(completed_capabilities_) + " named " + capabilities[completed_capabilities_].source.runner; + "Starting capability of Node " + std::to_string(completed_capabilities_) + " named " + capabilities[completed_capabilities_].source.runner; goal_handle->publish_feedback(feedback); RCLCPP_INFO(this->get_logger(), ""); RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); @@ -655,7 +655,7 @@ class CapabilitiesFabric : public rclcpp::Node auto response = future.get(); - feedback->progress = std::to_string(completed_capabilities_) + "/" + std::to_string(expected_capabilities_) + " : Capability use (start) succeessful"; + feedback->progress = std::to_string(completed_capabilities_) + "/" + std::to_string(expected_capabilities_) + " : Capability start succeessful"; goal_handle->publish_feedback(feedback); RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); diff --git a/capabilities2_fabric/include/capabilities2_executor/capabilities_xml_parser.hpp b/capabilities2_fabric/include/capabilities2_executor/capabilities_xml_parser.hpp index cd43df5..fedbc53 100644 --- a/capabilities2_fabric/include/capabilities2_executor/capabilities_xml_parser.hpp +++ b/capabilities2_fabric/include/capabilities2_executor/capabilities_xml_parser.hpp @@ -68,18 +68,18 @@ bool check_plan_tag(tinyxml2::XMLDocument& document) * * @return `true` if element is not nullptr and conversion successful, `false` if element is nullptr */ -bool convert_to_string(tinyxml2::XMLElement* element, std::string& paramters) +bool convert_to_string(tinyxml2::XMLElement* element, std::string& parameters) { if (element) { tinyxml2::XMLPrinter printer; element->Accept(&printer); - paramters = printer.CStr(); + parameters = printer.CStr(); return true; } else { - paramters = ""; + parameters = ""; return false; } } diff --git a/capabilities2_fabric/plans/navigation_1.xml b/capabilities2_fabric/plans/navigation_1.xml index 7e48c39..3b6de79 100644 --- a/capabilities2_fabric/plans/navigation_1.xml +++ b/capabilities2_fabric/plans/navigation_1.xml @@ -1,6 +1,6 @@ - + \ No newline at end of file diff --git a/capabilities2_fabric/plans/navigation_2.xml b/capabilities2_fabric/plans/navigation_2.xml index e87805d..a2569f7 100644 --- a/capabilities2_fabric/plans/navigation_2.xml +++ b/capabilities2_fabric/plans/navigation_2.xml @@ -1,7 +1,7 @@ - - + + \ No newline at end of file diff --git a/capabilities2_runner/include/capabilities2_runner/action_runner.hpp b/capabilities2_runner/include/capabilities2_runner/action_runner.hpp index f438e52..54429ee 100644 --- a/capabilities2_runner/include/capabilities2_runner/action_runner.hpp +++ b/capabilities2_runner/include/capabilities2_runner/action_runner.hpp @@ -88,11 +88,17 @@ class ActionRunner : public RunnerBase if (response->return_code != action_msgs::srv::CancelGoal_Response::ERROR_NONE) { // throw runner_exception("failed to cancel runner"); - RCLCPP_WARN(node_->get_logger(), "Runner cancelation failed."); + RCLCPP_WARN(node_->get_logger(), "Runner cancellation failed."); } // Trigger on_stopped event if defined - events[execute_id].on_stopped(update_on_stopped(events[execute_id].on_stopped_param)); + if (events[execute_id].on_stopped != "") + { + RCLCPP_INFO(node_->get_logger(), "[%s] on_stopped event available. Triggering", + run_config_.interface.c_str()); + triggerFunction_(events[execute_id].on_stopped, + update_on_stopped(events[execute_id].on_stopped_param)); + } }); // wait for action to be stopped. hold the thread for 2 seconds to help keep callbacks in scope @@ -118,34 +124,42 @@ class ActionRunner : public RunnerBase } /** - * @brief the trigger function on the action runner is used to trigger an action. - * this method provides a mechanism for injecting parameters or a goal into the action - * and then trigger the action + * @brief Trigger process to be executed. * - * @param parameters - * @return std::optional> + * This method utilizes paramters set via the trigger() function + * + * @param parameters pointer to tinyxml2::XMLElement that contains parameters */ - virtual std::optional> - trigger(tinyxml2::XMLElement* parameters = nullptr) override + virtual void triggerExecution() override { execute_id += 1; // if parameters are not provided then cannot proceed - if (!parameters) + if (!parameters_) throw runner_exception("cannot trigger action without parameters"); // generate a goal from parameters if provided - typename ActionT::Goal goal_msg = generate_goal(parameters); + typename ActionT::Goal goal_msg = generate_goal(parameters_); + + RCLCPP_INFO(node_->get_logger(), "[%s] goal generated.", run_config_.interface.c_str()); // send goal options // goal response callback send_goal_options_.goal_response_callback = [this](const typename rclcpp_action::ClientGoalHandle::SharedPtr& goal_handle) { if (goal_handle) - events[execute_id].on_started(update_on_started(events[execute_id].on_started_param)); + { + // trigger the events related to on_started state + if (events[execute_id].on_started != "") + { + RCLCPP_INFO(node_->get_logger(), "[%s] on_started event available. Triggering", + run_config_.interface.c_str()); + triggerFunction_(events[execute_id].on_started, update_on_started(events[execute_id].on_started_param)); + } + } else { - RCLCPP_ERROR(node_->get_logger(), "Goal was rejected by server"); + RCLCPP_ERROR(node_->get_logger(), "[%s] goal rejected", run_config_.interface.c_str()); } // store goal handle to be used with stop funtion @@ -155,10 +169,26 @@ class ActionRunner : public RunnerBase // result callback send_goal_options_.result_callback = [this](const typename rclcpp_action::ClientGoalHandle::WrappedResult& wrapped_result) { - if (wrapped_result.code == rclcpp_action::ResultCode::SUCCEEDED) - events[execute_id].on_success(update_on_success(events[execute_id].on_success_param)); // Trigger on_success event if defined - else - events[execute_id].on_failure(update_on_failure(events[execute_id].on_failure_param)); // Trigger on_failure event if defined + if (wrapped_result.code == rclcpp_action::ResultCode::SUCCEEDED) + { + // trigger the events related to on_success state + if (events[execute_id].on_success != "") + { + RCLCPP_INFO(node_->get_logger(), "[%s] on_success event available. Triggering", + run_config_.interface.c_str()); + triggerFunction_(events[execute_id].on_success, update_on_success(events[execute_id].on_success_param)); + } + } + else + { + // trigger the events related to on_failure state + if (events[execute_id].on_failure != "") + { + RCLCPP_INFO(node_->get_logger(), "[%s] on_failure event available. Triggering", + run_config_.interface.c_str()); + triggerFunction_(events[execute_id].on_failure, update_on_failure(events[execute_id].on_failure_param)); + } + } result_ = wrapped_result.result; }; @@ -166,33 +196,20 @@ class ActionRunner : public RunnerBase // trigger the action client with goal auto goal_handle_future = action_client_->async_send_goal(goal_msg, send_goal_options_); + RCLCPP_INFO(node_->get_logger(), "[%s] goal sent. Waiting for acceptance", run_config_.interface.c_str()); + // create a function to call for the result. the future will be returned to the caller and the caller // can provide a conversion function to handle the result - std::function result_callback = [this, - goal_handle_future](tinyxml2::XMLElement* result) { - auto goal_handle = goal_handle_future.get(); - - if (!goal_handle) - { - RCLCPP_ERROR(node_->get_logger(), "Failed to send goal"); - return; - } + auto goal_handle = goal_handle_future.get(); - // Get the result asynchronously - auto result_future = action_client_->async_get_result(goal_handle); + RCLCPP_INFO(node_->get_logger(), "[%s] goal accepted. Waiting for result", run_config_.interface.c_str()); - // Wait for result future - auto wrapped_result = result_future.get(); - - // convert the result - if (wrapped_result.code == rclcpp_action::ResultCode::SUCCEEDED) - { - result = generate_result(wrapped_result.result); - } - }; - - return result_callback; + if (!goal_handle) + { + RCLCPP_INFO(node_->get_logger(), "[%s] goal rejected", run_config_.interface.c_str()); + return; + } } protected: @@ -209,19 +226,6 @@ class ActionRunner : public RunnerBase */ virtual typename ActionT::Goal generate_goal(tinyxml2::XMLElement* parameters) = 0; - /** - * @brief generate a typed erased goal result - * - * this method is used in a callback passed to the trigger caller to get type erased result - * from the action the result can be passed by the caller or ignored - * - * The pattern needs to be implemented in the derived class - * - * @param wrapped_result - * @return tinyxml2::XMLElement* - */ - virtual tinyxml2::XMLElement* generate_result(const typename ActionT::Result::SharedPtr& result) = 0; - /**< action client */ typename rclcpp_action::Client::SharedPtr action_client_; diff --git a/capabilities2_runner/include/capabilities2_runner/launch_runner.hpp b/capabilities2_runner/include/capabilities2_runner/launch_runner.hpp index 82b32f4..22ff606 100644 --- a/capabilities2_runner/include/capabilities2_runner/launch_runner.hpp +++ b/capabilities2_runner/include/capabilities2_runner/launch_runner.hpp @@ -122,11 +122,18 @@ class LaunchRunner : public RunnerBase } // throw on trigger function - std::optional> trigger(tinyxml2::XMLElement* parameters) override + void trigger(const std::string& parameters) override { throw runner_exception("No Trigger as this is launch runner"); } +protected: + // throw on triggerExecution function + void triggerExecution() override + { + throw runner_exception("no triggerExecution() this is a no-trigger action runner"); + } + std::string launch_name; std::string package_name; diff --git a/capabilities2_runner/include/capabilities2_runner/notrigger_action_runner.hpp b/capabilities2_runner/include/capabilities2_runner/notrigger_action_runner.hpp index 0dc8a8b..de9c10b 100644 --- a/capabilities2_runner/include/capabilities2_runner/notrigger_action_runner.hpp +++ b/capabilities2_runner/include/capabilities2_runner/notrigger_action_runner.hpp @@ -18,22 +18,23 @@ class NoTriggerActionRunner : public ActionRunner { public: // throw on trigger function - std::optional> - trigger(tinyxml2::XMLElement* parameters) override + void trigger(tinyxml2::XMLElement* parameters) override { throw runner_exception("cannot trigger this is a no-trigger action runner"); } protected: - // throw on xml conversion functions - typename ActionT::Goal generate_goal(tinyxml2::XMLElement*) override + + // throw on triggerExecution function + void triggerExecution() override { - throw runner_exception("cannot generate goal this is a no-trigger action runner"); + throw runner_exception("no triggerExecution() this is a no-trigger action runner"); } - tinyxml2::XMLElement* generate_result(const typename ActionT::Result::SharedPtr& result) override + // throw on xml conversion functions + typename ActionT::Goal generate_goal(tinyxml2::XMLElement*) override { - throw runner_exception("cannot generate result this is a no-trigger action runner"); + throw runner_exception("cannot generate goal this is a no-trigger action runner"); } }; diff --git a/capabilities2_runner/include/capabilities2_runner/runner_base.hpp b/capabilities2_runner/include/capabilities2_runner/runner_base.hpp index 4027760..5a0db5e 100644 --- a/capabilities2_runner/include/capabilities2_runner/runner_base.hpp +++ b/capabilities2_runner/include/capabilities2_runner/runner_base.hpp @@ -3,7 +3,7 @@ #include #include #include -#include +#include #include #include @@ -63,25 +63,25 @@ struct runner_opts * * keeps track of events that are related to runner instances at various points of the * plan - * @param on_started pointer to function to execute on start - * @param on_success pointer to function to execute on success - * @param on_failure pointer to function to execute on failure - * @param on_stopped pointer to function to execute on stop - * @param on_started_param parameters for the function to execute on start - * @param on_success_param parameters for the function to execute on success - * @param on_failure_param parameters for the function to execute on failure - * @param on_stopped_param parameters for the function to execute on stop + * @param on_started name of the capability to execute on start + * @param on_success name of the capability to execute on success + * @param on_failure name of the capability to execute on failure + * @param on_stopped name of the capability to execute on stop + * @param on_started_param parameters for the capability to execute on start + * @param on_success_param parameters for the capability to execute on success + * @param on_failure_param parameters for the capability to execute on failure + * @param on_stopped_param parameters for the capability to execute on stop */ struct event_opts { - std::function on_started; - std::function on_success; - std::function on_failure; - std::function on_stopped; - tinyxml2::XMLElement* on_started_param; - tinyxml2::XMLElement* on_success_param; - tinyxml2::XMLElement* on_failure_param; - tinyxml2::XMLElement* on_stopped_param; + std::string on_started; + std::string on_success; + std::string on_failure; + std::string on_stopped; + std::string on_started_param; + std::string on_success_param; + std::string on_failure_param; + std::string on_stopped_param; }; class RunnerBase @@ -112,18 +112,20 @@ class RunnerBase virtual void stop() = 0; /** - * @brief trigger the runner + * @brief Trigger the runner * - * this method allows insertion of parameters in a runner after it has been initialized - * it is an approach to parameterise capabilities + * This method allows insertion of parameters in a runner after it has been initialized. it is an approach + * to parameterise capabilities. Internally starts up RunnerBase::triggerExecution in a thread * * @param parameters pointer to tinyxml2::XMLElement that contains parameters * - * @return std::optional)>> function pointer to invoke - * elsewhere such as an event callback */ - virtual std::optional> - trigger(tinyxml2::XMLElement* parameters = nullptr) = 0; + virtual void trigger(const std::string& parameters) + { + parameters_ = convert_to_xml(parameters); + + executionThread = std::thread(&RunnerBase::triggerExecution, this); + } /** * @brief Initializer function for initializing the base runner in place of constructor due to plugin semantics @@ -144,13 +146,17 @@ class RunnerBase * @brief attach events to the runner * * @param event_option event_options related for the action - * + * * @return number of attached events */ - int attach_events(capabilities2_runner::event_opts& event_option) + int attach_events(capabilities2_runner::event_opts& event_option, + std::function triggerFunction) { - RCLCPP_INFO(node_->get_logger(), "%s accepted event options with ID : %d ", run_config_.interface.c_str(), insert_id); - + RCLCPP_INFO(node_->get_logger(), "%s accepted event options with ID : %d ", run_config_.interface.c_str(), + insert_id); + + triggerFunction_ = triggerFunction; + events[insert_id] = event_option; insert_id += 1; @@ -198,6 +204,16 @@ class RunnerBase } protected: + /** + * @brief Trigger process to be executed. + * + * This method utilizes paramters set via the trigger() function + * + * @param parameters pointer to tinyxml2::XMLElement that contains parameters + * + */ + virtual void triggerExecution() = 0; + /** * @brief Update on_started event parameters with new data if avaible. * @@ -209,7 +225,7 @@ class RunnerBase * @param parameters pointer to the XMLElement containing parameters * @return pointer to the XMLElement containing updated parameters */ - virtual tinyxml2::XMLElement* update_on_started(tinyxml2::XMLElement* parameters) + virtual std::string update_on_started(std::string& parameters) { return parameters; }; @@ -225,7 +241,7 @@ class RunnerBase * @param parameters pointer to the XMLElement containing parameters * @return pointer to the XMLElement containing updated parameters */ - virtual tinyxml2::XMLElement* update_on_stopped(tinyxml2::XMLElement* parameters) + virtual std::string update_on_stopped(std::string& parameters) { return parameters; }; @@ -241,7 +257,7 @@ class RunnerBase * @param parameters pointer to the XMLElement containing parameters * @return pointer to the XMLElement containing updated parameters */ - virtual tinyxml2::XMLElement* update_on_failure(tinyxml2::XMLElement* parameters) + virtual std::string update_on_failure(std::string& parameters) { return parameters; }; @@ -257,11 +273,55 @@ class RunnerBase * @param parameters pointer to the XMLElement containing parameters * @return pointer to the XMLElement containing updated parameters */ - virtual tinyxml2::XMLElement* update_on_success(tinyxml2::XMLElement* parameters) + virtual std::string update_on_success(std::string& parameters) { return parameters; }; + /** + * @brief convert an XMLElement to std::string + * + * @param element XMLElement element to be converted + * @param paramters parameter to hold std::string + * + * @return `true` if element is not nullptr and conversion successful, `false` if element is nullptr + */ + std::string convert_to_string(tinyxml2::XMLElement* element) + { + if (element) + { + element->Accept(&printer); + std::string parameters = printer.CStr(); + return parameters; + } + else + { + std::string parameters = ""; + return parameters; + } + } + + /** + * @brief convert an XMLElement to std::string + * + * @param element XMLElement element to be converted + * @param paramters parameter to hold std::string + * + * @return `true` if element is not nullptr and conversion successful, `false` if element is nullptr + */ + tinyxml2::XMLElement* convert_to_xml(const std::string& parameters) + { + if (parameters != "") + { + doc.Parse(parameters.c_str()); + tinyxml2::XMLElement* element = doc.FirstChildElement(); + return element; + } + else + { + return nullptr; + } + } // run config getters /** @@ -423,8 +483,28 @@ class RunnerBase /** * @brief pointer to XMLElement which contain parameters - * */ + */ tinyxml2::XMLElement* parameters_; + + /** + * @brief thread that executes the triggerExecution functionality + */ + std::thread executionThread; + + /** + * @brief external function that triggers capability runners + */ + std::function triggerFunction_; + + /** + * @brief XMLElement that is used to convert xml strings to std::string + */ + tinyxml2::XMLPrinter printer; + + /** + * @brief XMLElement that is used to convert std::string to xml strings + */ + tinyxml2::XMLDocument doc; }; } // namespace capabilities2_runner diff --git a/capabilities2_runner/include/capabilities2_runner/service_runner.hpp b/capabilities2_runner/include/capabilities2_runner/service_runner.hpp index 73201d8..5058784 100644 --- a/capabilities2_runner/include/capabilities2_runner/service_runner.hpp +++ b/capabilities2_runner/include/capabilities2_runner/service_runner.hpp @@ -52,61 +52,54 @@ class ServiceRunner : public RunnerBase } /** - * @brief the trigger function on the service runner is used to trigger an service. - * this method provides a mechanism for injecting parameters or a goal into a service - * and then trigger the service + * @brief Trigger process to be executed. * - * @param parameters - * @return std::optional> + * This method utilizes paramters set via the trigger() function + * + * @param parameters pointer to tinyxml2::XMLElement that contains parameters */ - virtual std::optional> - trigger(tinyxml2::XMLElement* parameters = nullptr) override + virtual void triggerExecution() override { execute_id += 1; // if parameters are not provided then cannot proceed - if (!parameters) + if (!parameters_) throw runner_exception("cannot trigger service without parameters"); // generate a goal from parameters if provided - auto request_msg = std::make_shared(generate_request(parameters)); + auto request_msg = std::make_shared(generate_request(parameters_)); auto result_future = service_client_->async_send_request( request_msg, [this](typename rclcpp::Client::SharedFuture future) { if (!future.valid()) { RCLCPP_ERROR(node_->get_logger(), "get result call failed"); - events[execute_id].on_failure(update_on_failure(events[execute_id].on_failure_param)); + + // trigger the events related to on_failure state + if (events[execute_id].on_failure != "") + { + triggerFunction_(events[execute_id].on_failure, update_on_failure(events[execute_id].on_failure_param)); + } } else { RCLCPP_INFO(node_->get_logger(), "get result call succeeded"); - events[execute_id].on_success(update_on_success(events[execute_id].on_success_param)); - } - }); - - // Trigger started event if defined - events[execute_id].on_started(update_on_started(events[execute_id].on_started_param)); - // Define a callback function to handle the result once it's ready - std::function result_callback = - [this, &result_future](tinyxml2::XMLElement* result) mutable { - auto response = result_future.get(); - result = generate_response(response); + response_ = future.get(); - // Ensure the future is ready before accessing the result - if (result_future.wait_for(std::chrono::seconds(0)) == std::future_status::ready) - { - auto response = result_future.get(); - result = generate_response(response); + // trigger the events related to on_success state + if (events[execute_id].on_success != "") + { + triggerFunction_(events[execute_id].on_success, update_on_success(events[execute_id].on_success_param)); + } } - else - { - RCLCPP_WARN(node_->get_logger(), "Result is not ready yet."); - } - }; + }); - return result_callback; + // trigger the events related to on_started state + if (events[execute_id].on_started != "") + { + triggerFunction_(events[execute_id].on_started, update_on_started(events[execute_id].on_started_param)); + } } /** @@ -127,8 +120,11 @@ class ServiceRunner : public RunnerBase if (!service_client_) throw runner_exception("cannot stop runner action that was not started"); - // publish event - events[execute_id].on_stopped(update_on_stopped(events[execute_id].on_stopped_param)); + // Trigger on_stopped event if defined + if (events[execute_id].on_stopped != "") + { + triggerFunction_(events[execute_id].on_stopped, update_on_stopped(events[execute_id].on_stopped_param)); + } } protected: @@ -145,19 +141,7 @@ class ServiceRunner : public RunnerBase */ virtual typename ServiceT::Request generate_request(tinyxml2::XMLElement* parameters) = 0; - /** - * @brief generate a typed erased response - * - * This method is used in a callback passed to the trigger caller to get type erased result - * from the service the reponse can be passed by the caller or ignored - * - * The pattern needs to be implemented in the derived class - * - * @param wrapped_result - * @return tinyxml2::XMLElement* - */ - virtual tinyxml2::XMLElement* generate_response(const typename ServiceT::Response::SharedPtr& result) const = 0; - typename rclcpp::Client::SharedPtr service_client_; + typename ServiceT::Response::SharedPtr response_; }; } // namespace capabilities2_runner \ No newline at end of file diff --git a/capabilities2_runner/include/capabilities2_runner/topic_runner.hpp b/capabilities2_runner/include/capabilities2_runner/topic_runner.hpp index 266d22c..64c26c3 100644 --- a/capabilities2_runner/include/capabilities2_runner/topic_runner.hpp +++ b/capabilities2_runner/include/capabilities2_runner/topic_runner.hpp @@ -44,41 +44,44 @@ class TopicRunner : public RunnerBase } /** - * @brief the trigger function on the topic subscriber runner is used to trigger data retrival. + * @brief Trigger process to be executed. * - * @param parameters - * @return std::optional> + * This method utilizes paramters set via the trigger() function + * + * @param parameters pointer to tinyxml2::XMLElement that contains parameters */ - virtual std::optional> - trigger(tinyxml2::XMLElement* parameters = nullptr) override + virtual void triggerExecution() override { execute_id += 1; // if parameters are not provided then cannot proceed - if (!parameters) + if (!parameters_) throw runner_exception("cannot grab data without parameters"); - events[execute_id].on_started(update_on_started(events[execute_id].on_started_param)); + // trigger the events related to on_started state + if (events[execute_id].on_started != "") + { + triggerFunction_(events[execute_id].on_started, update_on_started(events[execute_id].on_started_param)); + } if (latest_message_) { - events[execute_id].on_success(update_on_success(events[execute_id].on_success_param)); + // trigger the events related to on_success state + if (events[execute_id].on_success != "") + { + triggerFunction_(events[execute_id].on_success, update_on_success(events[execute_id].on_success_param)); + } } else { RCLCPP_ERROR(node_->get_logger(), "get result call failed"); - events[execute_id].on_failure(update_on_failure(events[execute_id].on_failure_param)); - } - // create a function to call for the result. the future will be returned to the caller and the caller - // can provide a conversion function to handle the result - - std::function result_callback = [this](tinyxml2::XMLElement* result) { - if (latest_message_) - result = generate_message(latest_message_); - }; - - return result_callback; + // trigger the events related to on_failure state + if (events[execute_id].on_failure != "") + { + triggerFunction_(events[execute_id].on_failure, update_on_failure(events[execute_id].on_failure_param)); + } + } } /** @@ -99,8 +102,11 @@ class TopicRunner : public RunnerBase if (!subscription_) throw runner_exception("cannot stop runner subscriber that was not started"); - // publish event - events[execute_id].on_stopped(update_on_stopped(events[execute_id].on_stopped_param)); + // Trigger on_stopped event if defined + if (events[execute_id].on_stopped != "") + { + triggerFunction_(events[execute_id].on_stopped, update_on_stopped(events[execute_id].on_stopped_param)); + } } protected: @@ -117,19 +123,6 @@ class TopicRunner : public RunnerBase latest_message_ = msg; } - /** - * @brief generate xml data structure of the message - * - * this method is used in a callback passed to the trigger caller to get type erased result - * of the latest message can be passed by the caller or ignored - * - * The pattern needs to be implemented in the derived class - * - * @param wrapped_result - * @return tinyxml2::XMLElement* - */ - virtual tinyxml2::XMLElement* generate_message(typename TopicT::SharedPtr& result) = 0; - typename rclcpp::Subscription::SharedPtr subscription_; mutable typename TopicT::SharedPtr latest_message_{ nullptr }; diff --git a/capabilities2_runner/src/standard_runners.cpp b/capabilities2_runner/src/standard_runners.cpp index 88924a5..c01054d 100644 --- a/capabilities2_runner/src/standard_runners.cpp +++ b/capabilities2_runner/src/standard_runners.cpp @@ -26,11 +26,16 @@ class DummyRunner : public RunnerBase RCLCPP_INFO(node_->get_logger(), "Dummy runner stopped"); } - std::optional> - trigger(tinyxml2::XMLElement* parameters) override + void trigger(const std::string& parameters) override { RCLCPP_INFO(node_->get_logger(), "Dummy runner cannot trigger"); - return std::nullopt; + } + +protected: + // throw on triggerExecution function + void triggerExecution() override + { + RCLCPP_INFO(node_->get_logger(), "Dummy runner does not have triggerExecution()"); } }; 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 f507939..59d2e8a 100644 --- a/capabilities2_runner_audio/include/capabilities2_runner_audio/listen_runner.hpp +++ b/capabilities2_runner_audio/include/capabilities2_runner_audio/listen_runner.hpp @@ -55,25 +55,6 @@ class ListenerRunner : public ActionRunner return goal_msg; } - /** - * @brief This generate_result function overrides the generate_result() function from ActionRunner(). Since - * - * @param result message from SpeechToText action - * @return tinyxml2::XMLElement* in the format '' - */ - virtual tinyxml2::XMLElement* - generate_result(const hri_audio_msgs::action::SpeechToText::Result::SharedPtr& result) override - { - tinyxml2::XMLDocument doc; - - // Root element - tinyxml2::XMLElement* textElement = doc.NewElement("Text"); - doc.InsertEndChild(textElement); - textElement->SetText(result->stt_text.c_str()); - - return doc.FirstChildElement("Text"); - } - /** * @brief Update on_success event parameters with new data if avaible. * @@ -85,15 +66,19 @@ class ListenerRunner : public ActionRunner * @param parameters pointer to the XMLElement containing parameters * @return pointer to the XMLElement containing updated parameters */ - virtual tinyxml2::XMLElement* update_on_success(tinyxml2::XMLElement* parameters) + virtual std::string update_on_success(std::string& parameters) { + tinyxml2::XMLElement* element = convert_to_xml(parameters); + // Create the Pose element as a child of the existing parameters element - tinyxml2::XMLElement* textElement = parameters->GetDocument()->NewElement("Text"); - parameters->InsertEndChild(textElement); + tinyxml2::XMLElement* textElement = element->GetDocument()->NewElement("Text"); + element->InsertEndChild(textElement); textElement->SetText(result_->stt_text.c_str()); - // Return the updated parameters element with Pose added - return parameters; + // Return the updated parameters element with Pose added as string + std::string result = convert_to_string(element); + + return result; }; }; diff --git a/capabilities2_runner_audio/include/capabilities2_runner_audio/speak_runner.hpp b/capabilities2_runner_audio/include/capabilities2_runner_audio/speak_runner.hpp index d8f37bc..925d90f 100644 --- a/capabilities2_runner_audio/include/capabilities2_runner_audio/speak_runner.hpp +++ b/capabilities2_runner_audio/include/capabilities2_runner_audio/speak_runner.hpp @@ -59,19 +59,6 @@ class SpeakerRunner : public ActionRunner return goal_msg; } - /** - * @brief This generate_result function overrides the generate_result() function from ActionRunner(). Since this does not - * return a value, return value is nullptr - * - * @param result message from SpeechToText action - * @return nullptr - */ - virtual tinyxml2::XMLElement* - generate_result(const hri_audio_msgs::action::TextToSpeech::Result::SharedPtr& result) override - { - return nullptr; - } - }; } // namespace capabilities2_runner \ No newline at end of file diff --git a/capabilities2_runner_bt/include/capabilities2_runner_bt/bt_factory_runner.hpp b/capabilities2_runner_bt/include/capabilities2_runner_bt/bt_factory_runner.hpp index b26ebc0..ff3b463 100644 --- a/capabilities2_runner_bt/include/capabilities2_runner_bt/bt_factory_runner.hpp +++ b/capabilities2_runner_bt/include/capabilities2_runner_bt/bt_factory_runner.hpp @@ -21,13 +21,10 @@ class BTRunnerBase : public RunnerBase, public BT::BehaviorTreeFactory { } - virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config, - std::function on_started = nullptr, - std::function on_terminated = nullptr, - std::function on_stopped = nullptr) + virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config) { // init the runner base - init_base(node, run_config, on_started, on_terminated, on_stopped); + init_base(node, run_config); // register (bt)actions from ROS plugins try @@ -46,14 +43,14 @@ class BTRunnerBase : public RunnerBase, public BT::BehaviorTreeFactory tree_.reset(); } +protected: /** * @brief trigger the behaviour factory with the input data * * @param parameters * @return std::optional)>> */ - virtual std::optional)>> - trigger(std::shared_ptr parameters) + virtual void trigger(tinyxml2::XMLElement* parameters = nullptr) override { // if parameters are not provided then cannot proceed if (!parameters) @@ -63,25 +60,24 @@ class BTRunnerBase : public RunnerBase, public BT::BehaviorTreeFactory tree_ = std::make_shared(this->createTreeFromText(parameters->GetText())); // return the tick function - // the caller can call this function to tick the tree - std::function)> result_callback = - [this](std::shared_ptr result) { - // tick the tree - BT::NodeStatus status = tree_->tickWhileRunning(); + // // the caller can call this function to tick the tree + // std::function)> result_callback = + // [this](std::shared_ptr result) { + // // tick the tree + // BT::NodeStatus status = tree_->tickWhileRunning(); - // fill the result - if (status == BT::NodeStatus::SUCCESS) - result->SetText("OK"); - else - result->SetText("FAIL"); + // // fill the result + // if (status == BT::NodeStatus::SUCCESS) + // result->SetText("OK"); + // else + // result->SetText("FAIL"); - return; - }; + // return; + // }; - return result_callback; + // return result_callback; } -protected: // the tree std::shared_ptr tree_; }; diff --git a/capabilities2_runner_nav2/include/capabilities2_runner_nav2/occupancygrid_runner.hpp b/capabilities2_runner_nav2/include/capabilities2_runner_nav2/occupancygrid_runner.hpp index f5f0ef3..6229a76 100644 --- a/capabilities2_runner_nav2/include/capabilities2_runner_nav2/occupancygrid_runner.hpp +++ b/capabilities2_runner_nav2/include/capabilities2_runner_nav2/occupancygrid_runner.hpp @@ -35,122 +35,6 @@ class OccupancyGridRunner : public TopicRunner } protected: - /** - * @brief This generate goal function overrides the generate_message() function from TopicRunner() - * @param result of the type geometry_msgs::msg::PoseWithCovarianceStamped - - * @return tinyxml2::XMLElement* of the robot's pose - */ - virtual tinyxml2::XMLElement* - generate_message(typename nav_msgs::msg::OccupancyGrid::SharedPtr& result) override - { - tinyxml2::XMLDocument doc; - - // Root element for OccupancyGrid - tinyxml2::XMLElement* occupancyGridElement = doc.NewElement("OccupancyGrid"); - doc.InsertFirstChild(occupancyGridElement); - - // Header element - tinyxml2::XMLElement* headerElement = doc.NewElement("header"); - occupancyGridElement->InsertEndChild(headerElement); - - // Header fields: stamp, frame_id - tinyxml2::XMLElement* stampElement = doc.NewElement("stamp"); - headerElement->InsertEndChild(stampElement); - - tinyxml2::XMLElement* secsElement = doc.NewElement("secs"); - secsElement->SetText(result->header.stamp.sec); - stampElement->InsertEndChild(secsElement); - - tinyxml2::XMLElement* nsecsElement = doc.NewElement("nsecs"); - nsecsElement->SetText(result->header.stamp.nanosec); - stampElement->InsertEndChild(nsecsElement); - - tinyxml2::XMLElement* frameIdElement = doc.NewElement("frame_id"); - frameIdElement->SetText(result->header.frame_id.c_str()); - headerElement->InsertEndChild(frameIdElement); - - // Info element - tinyxml2::XMLElement* infoElement = doc.NewElement("info"); - occupancyGridElement->InsertEndChild(infoElement); - - // Map info fields: resolution, width, height - tinyxml2::XMLElement* resolutionElement = doc.NewElement("resolution"); - resolutionElement->SetText(result->info.resolution); - infoElement->InsertEndChild(resolutionElement); - - tinyxml2::XMLElement* widthElement = doc.NewElement("width"); - widthElement->SetText(result->info.width); - infoElement->InsertEndChild(widthElement); - - tinyxml2::XMLElement* heightElement = doc.NewElement("height"); - heightElement->SetText(result->info.height); - infoElement->InsertEndChild(heightElement); - - // Origin position element - tinyxml2::XMLElement* originElement = doc.NewElement("origin"); - infoElement->InsertEndChild(originElement); - - tinyxml2::XMLElement* positionElement = doc.NewElement("position"); - originElement->InsertEndChild(positionElement); - - tinyxml2::XMLElement* posX = doc.NewElement("x"); - posX->SetText(result->info.origin.position.x); - positionElement->InsertEndChild(posX); - - tinyxml2::XMLElement* posY = doc.NewElement("y"); - posY->SetText(result->info.origin.position.y); - positionElement->InsertEndChild(posY); - - tinyxml2::XMLElement* posZ = doc.NewElement("z"); - posZ->SetText(result->info.origin.position.z); - positionElement->InsertEndChild(posZ); - - // Origin orientation element - tinyxml2::XMLElement* orientationElement = doc.NewElement("orientation"); - originElement->InsertEndChild(orientationElement); - - tinyxml2::XMLElement* oriX = doc.NewElement("x"); - oriX->SetText(result->info.origin.orientation.x); - orientationElement->InsertEndChild(oriX); - - tinyxml2::XMLElement* oriY = doc.NewElement("y"); - oriY->SetText(result->info.origin.orientation.y); - orientationElement->InsertEndChild(oriY); - - tinyxml2::XMLElement* oriZ = doc.NewElement("z"); - oriZ->SetText(result->info.origin.orientation.z); - orientationElement->InsertEndChild(oriZ); - - tinyxml2::XMLElement* oriW = doc.NewElement("w"); - oriW->SetText(result->info.origin.orientation.w); - orientationElement->InsertEndChild(oriW); - - // Map load time - tinyxml2::XMLElement* mapLoadTimeElement = doc.NewElement("map_load_time"); - infoElement->InsertEndChild(mapLoadTimeElement); - - tinyxml2::XMLElement* mapSecsElement = doc.NewElement("secs"); - mapSecsElement->SetText(result->info.map_load_time.sec); - mapLoadTimeElement->InsertEndChild(mapSecsElement); - - tinyxml2::XMLElement* mapNsecsElement = doc.NewElement("nsecs"); - mapNsecsElement->SetText(result->info.map_load_time.nanosec); - mapLoadTimeElement->InsertEndChild(mapNsecsElement); - - // Data element for occupancy grid data - tinyxml2::XMLElement* dataElement = doc.NewElement("data"); - occupancyGridElement->InsertEndChild(dataElement); - - // Convert the occupancy grid data into a space-separated string - std::string data_str; - for (const auto& value : result->data) { - data_str += std::to_string(value) + " "; - } - dataElement->SetText(data_str.c_str()); - - return doc.FirstChildElement("OccupancyGrid"); - } /** * @brief Update on_success event parameters with new data if avaible. @@ -163,99 +47,101 @@ class OccupancyGridRunner : public TopicRunner * @param parameters pointer to the XMLElement containing parameters * @return pointer to the XMLElement containing updated parameters */ - virtual tinyxml2::XMLElement* update_on_success(tinyxml2::XMLElement* parameters) + virtual std::string update_on_success(std::string& parameters) { + tinyxml2::XMLElement* element = convert_to_xml(parameters); + // Create the OccupancyGrid element as a child of the existing parameters element - tinyxml2::XMLElement* occupancyGridElement = parameters->GetDocument()->NewElement("OccupancyGrid"); - parameters->InsertEndChild(occupancyGridElement); + tinyxml2::XMLElement* occupancyGridElement = element->GetDocument()->NewElement("OccupancyGrid"); + element->InsertEndChild(occupancyGridElement); // Header element - tinyxml2::XMLElement* headerElement = parameters->GetDocument()->NewElement("header"); + tinyxml2::XMLElement* headerElement = element->GetDocument()->NewElement("header"); occupancyGridElement->InsertEndChild(headerElement); - tinyxml2::XMLElement* stampElement = parameters->GetDocument()->NewElement("stamp"); + tinyxml2::XMLElement* stampElement = element->GetDocument()->NewElement("stamp"); headerElement->InsertEndChild(stampElement); - tinyxml2::XMLElement* secsElement = parameters->GetDocument()->NewElement("secs"); + tinyxml2::XMLElement* secsElement = element->GetDocument()->NewElement("secs"); secsElement->SetText(latest_message_->header.stamp.sec); stampElement->InsertEndChild(secsElement); - tinyxml2::XMLElement* nsecsElement = parameters->GetDocument()->NewElement("nsecs"); + tinyxml2::XMLElement* nsecsElement = element->GetDocument()->NewElement("nsecs"); nsecsElement->SetText(latest_message_->header.stamp.nanosec); stampElement->InsertEndChild(nsecsElement); - tinyxml2::XMLElement* frameIdElement = parameters->GetDocument()->NewElement("frame_id"); + tinyxml2::XMLElement* frameIdElement = element->GetDocument()->NewElement("frame_id"); frameIdElement->SetText(latest_message_->header.frame_id.c_str()); headerElement->InsertEndChild(frameIdElement); // Info element - tinyxml2::XMLElement* infoElement = parameters->GetDocument()->NewElement("info"); + tinyxml2::XMLElement* infoElement = element->GetDocument()->NewElement("info"); occupancyGridElement->InsertEndChild(infoElement); - tinyxml2::XMLElement* resolutionElement = parameters->GetDocument()->NewElement("resolution"); + tinyxml2::XMLElement* resolutionElement = element->GetDocument()->NewElement("resolution"); resolutionElement->SetText(latest_message_->info.resolution); infoElement->InsertEndChild(resolutionElement); - tinyxml2::XMLElement* widthElement = parameters->GetDocument()->NewElement("width"); + tinyxml2::XMLElement* widthElement = element->GetDocument()->NewElement("width"); widthElement->SetText(latest_message_->info.width); infoElement->InsertEndChild(widthElement); - tinyxml2::XMLElement* heightElement = parameters->GetDocument()->NewElement("height"); + tinyxml2::XMLElement* heightElement = element->GetDocument()->NewElement("height"); heightElement->SetText(latest_message_->info.height); infoElement->InsertEndChild(heightElement); // Origin element - tinyxml2::XMLElement* originElement = parameters->GetDocument()->NewElement("origin"); + tinyxml2::XMLElement* originElement = element->GetDocument()->NewElement("origin"); infoElement->InsertEndChild(originElement); - tinyxml2::XMLElement* positionElement = parameters->GetDocument()->NewElement("position"); + tinyxml2::XMLElement* positionElement = element->GetDocument()->NewElement("position"); originElement->InsertEndChild(positionElement); - tinyxml2::XMLElement* posX = parameters->GetDocument()->NewElement("x"); + tinyxml2::XMLElement* posX = element->GetDocument()->NewElement("x"); posX->SetText(latest_message_->info.origin.position.x); positionElement->InsertEndChild(posX); - tinyxml2::XMLElement* posY = parameters->GetDocument()->NewElement("y"); + tinyxml2::XMLElement* posY = element->GetDocument()->NewElement("y"); posY->SetText(latest_message_->info.origin.position.y); positionElement->InsertEndChild(posY); - tinyxml2::XMLElement* posZ = parameters->GetDocument()->NewElement("z"); + tinyxml2::XMLElement* posZ = element->GetDocument()->NewElement("z"); posZ->SetText(latest_message_->info.origin.position.z); positionElement->InsertEndChild(posZ); - tinyxml2::XMLElement* orientationElement = parameters->GetDocument()->NewElement("orientation"); + tinyxml2::XMLElement* orientationElement = element->GetDocument()->NewElement("orientation"); originElement->InsertEndChild(orientationElement); - tinyxml2::XMLElement* oriX = parameters->GetDocument()->NewElement("x"); + tinyxml2::XMLElement* oriX = element->GetDocument()->NewElement("x"); oriX->SetText(latest_message_->info.origin.orientation.x); orientationElement->InsertEndChild(oriX); - tinyxml2::XMLElement* oriY = parameters->GetDocument()->NewElement("y"); + tinyxml2::XMLElement* oriY = element->GetDocument()->NewElement("y"); oriY->SetText(latest_message_->info.origin.orientation.y); orientationElement->InsertEndChild(oriY); - tinyxml2::XMLElement* oriZ = parameters->GetDocument()->NewElement("z"); + tinyxml2::XMLElement* oriZ = element->GetDocument()->NewElement("z"); oriZ->SetText(latest_message_->info.origin.orientation.z); orientationElement->InsertEndChild(oriZ); - tinyxml2::XMLElement* oriW = parameters->GetDocument()->NewElement("w"); + tinyxml2::XMLElement* oriW = element->GetDocument()->NewElement("w"); oriW->SetText(latest_message_->info.origin.orientation.w); orientationElement->InsertEndChild(oriW); // Map load time - tinyxml2::XMLElement* mapLoadTimeElement = parameters->GetDocument()->NewElement("map_load_time"); + tinyxml2::XMLElement* mapLoadTimeElement = element->GetDocument()->NewElement("map_load_time"); infoElement->InsertEndChild(mapLoadTimeElement); - tinyxml2::XMLElement* mapSecsElement = parameters->GetDocument()->NewElement("secs"); + tinyxml2::XMLElement* mapSecsElement = element->GetDocument()->NewElement("secs"); mapSecsElement->SetText(latest_message_->info.map_load_time.sec); mapLoadTimeElement->InsertEndChild(mapSecsElement); - tinyxml2::XMLElement* mapNsecsElement = parameters->GetDocument()->NewElement("nsecs"); + tinyxml2::XMLElement* mapNsecsElement = element->GetDocument()->NewElement("nsecs"); mapNsecsElement->SetText(latest_message_->info.map_load_time.nanosec); mapLoadTimeElement->InsertEndChild(mapNsecsElement); // Data element for occupancy data (converted to a string) - tinyxml2::XMLElement* dataElement = parameters->GetDocument()->NewElement("data"); + tinyxml2::XMLElement* dataElement = element->GetDocument()->NewElement("data"); occupancyGridElement->InsertEndChild(dataElement); std::string data_str; @@ -265,7 +151,9 @@ class OccupancyGridRunner : public TopicRunner dataElement->SetText(data_str.c_str()); // Return the updated parameters element with OccupancyGrid added - return parameters; + std::string result = convert_to_string(element); + + return result; }; }; } // namespace capabilities2_runner \ No newline at end of file diff --git a/capabilities2_runner_nav2/include/capabilities2_runner_nav2/robotpose_runner.hpp b/capabilities2_runner_nav2/include/capabilities2_runner_nav2/robotpose_runner.hpp index 83904d7..763d5a5 100644 --- a/capabilities2_runner_nav2/include/capabilities2_runner_nav2/robotpose_runner.hpp +++ b/capabilities2_runner_nav2/include/capabilities2_runner_nav2/robotpose_runner.hpp @@ -35,62 +35,6 @@ class RobotPoseRunner : public TopicRunnerInsertEndChild(positionElement); - - // Position x, y, z - tinyxml2::XMLElement* posX = doc.NewElement("x"); - posX->SetText(result->pose.pose.position.x); // Set x position value - positionElement->InsertEndChild(posX); - - tinyxml2::XMLElement* posY = doc.NewElement("y"); - posY->SetText(result->pose.pose.position.y); // Set y position value - positionElement->InsertEndChild(posY); - - tinyxml2::XMLElement* posZ = doc.NewElement("z"); - posZ->SetText(result->pose.pose.position.z); // Set z position value - positionElement->InsertEndChild(posZ); - - // Orientation element - tinyxml2::XMLElement* orientationElement = doc.NewElement("orientation"); - poseElement->InsertEndChild(orientationElement); - - // Orientation x, y, z, w - tinyxml2::XMLElement* oriX = doc.NewElement("x"); - oriX->SetText(result->pose.pose.orientation.x); // Set orientation x value - orientationElement->InsertEndChild(oriX); - - tinyxml2::XMLElement* oriY = doc.NewElement("y"); - oriY->SetText(result->pose.pose.orientation.y); // Set orientation y value - orientationElement->InsertEndChild(oriY); - - tinyxml2::XMLElement* oriZ = doc.NewElement("z"); - oriZ->SetText(result->pose.pose.orientation.z); // Set orientation z value - orientationElement->InsertEndChild(oriZ); - - tinyxml2::XMLElement* oriW = doc.NewElement("w"); - oriW->SetText(result->pose.pose.orientation.w); // Set orientation w value - orientationElement->InsertEndChild(oriW); - - return doc.FirstChildElement("Pose"); - } - /** * @brief Update on_success event parameters with new data if avaible. * @@ -102,52 +46,56 @@ class RobotPoseRunner : public TopicRunnerGetDocument()->NewElement("Pose"); - parameters->InsertEndChild(poseElement); + tinyxml2::XMLElement* poseElement = element->GetDocument()->NewElement("Pose"); + element->InsertEndChild(poseElement); // Position element - tinyxml2::XMLElement* positionElement = parameters->GetDocument()->NewElement("position"); + tinyxml2::XMLElement* positionElement = element->GetDocument()->NewElement("position"); poseElement->InsertEndChild(positionElement); // Position x, y, z - tinyxml2::XMLElement* posX = parameters->GetDocument()->NewElement("x"); + tinyxml2::XMLElement* posX = element->GetDocument()->NewElement("x"); posX->SetText(latest_message_->pose.pose.position.x); // Set x position value positionElement->InsertEndChild(posX); - tinyxml2::XMLElement* posY = parameters->GetDocument()->NewElement("y"); + tinyxml2::XMLElement* posY = element->GetDocument()->NewElement("y"); posY->SetText(latest_message_->pose.pose.position.y); // Set y position value positionElement->InsertEndChild(posY); - tinyxml2::XMLElement* posZ = parameters->GetDocument()->NewElement("z"); + tinyxml2::XMLElement* posZ = element->GetDocument()->NewElement("z"); posZ->SetText(latest_message_->pose.pose.position.z); // Set z position value positionElement->InsertEndChild(posZ); // Orientation element - tinyxml2::XMLElement* orientationElement = parameters->GetDocument()->NewElement("orientation"); + tinyxml2::XMLElement* orientationElement = element->GetDocument()->NewElement("orientation"); poseElement->InsertEndChild(orientationElement); // Orientation x, y, z, w - tinyxml2::XMLElement* oriX = parameters->GetDocument()->NewElement("x"); + tinyxml2::XMLElement* oriX = element->GetDocument()->NewElement("x"); oriX->SetText(latest_message_->pose.pose.orientation.x); // Set orientation x value orientationElement->InsertEndChild(oriX); - tinyxml2::XMLElement* oriY = parameters->GetDocument()->NewElement("y"); + tinyxml2::XMLElement* oriY = element->GetDocument()->NewElement("y"); oriY->SetText(latest_message_->pose.pose.orientation.y); // Set orientation y value orientationElement->InsertEndChild(oriY); - tinyxml2::XMLElement* oriZ = parameters->GetDocument()->NewElement("z"); + tinyxml2::XMLElement* oriZ = element->GetDocument()->NewElement("z"); oriZ->SetText(latest_message_->pose.pose.orientation.z); // Set orientation z value orientationElement->InsertEndChild(oriZ); - tinyxml2::XMLElement* oriW = parameters->GetDocument()->NewElement("w"); + tinyxml2::XMLElement* oriW = element->GetDocument()->NewElement("w"); oriW->SetText(latest_message_->pose.pose.orientation.w); // Set orientation w value orientationElement->InsertEndChild(oriW); - // Return the updated parameters element with Pose added - return parameters; + // Return the updated parameters element with Pose added as string + std::string result = convert_to_string(element); + + return result; }; }; } // namespace capabilities2_runner \ No newline at end of file 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 fbad70b..8d30bba 100644 --- a/capabilities2_runner_nav2/include/capabilities2_runner_nav2/waypoint_runner.hpp +++ b/capabilities2_runner_nav2/include/capabilities2_runner_nav2/waypoint_runner.hpp @@ -52,7 +52,7 @@ class WayPointRunner : public ActionRunner parameters_->QueryDoubleAttribute("x", &x); parameters_->QueryDoubleAttribute("y", &y); - RCLCPP_INFO(node_->get_logger(), "%s is triggered with x: %f and y: %f", run_config_.interface.c_str(), x, y); + RCLCPP_INFO(node_->get_logger(), "[%s] goal consist of x: %f and y: %f", run_config_.interface.c_str(), x, y); nav2_msgs::action::NavigateToPose::Goal goal_msg; geometry_msgs::msg::PoseStamped pose_msg; @@ -68,18 +68,6 @@ class WayPointRunner : public ActionRunner return goal_msg; } - /** - * @brief This generate result function overrides the generate_result() function from ActionRunner(). Since - * this is not used in this context, this returns nullptr - * @param result message from FollowWaypoints action - * @return nullptr - */ - virtual tinyxml2::XMLElement* - generate_result(const nav2_msgs::action::NavigateToPose::Result::SharedPtr& result) override - { - return nullptr; - } - protected: std::string global_frame_; /**The global frame of the robot*/ std::string robot_base_frame_; /**The frame of the robot base*/ diff --git a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_occupancy_runner.hpp b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_occupancy_runner.hpp index f3086f2..709f90d 100644 --- a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_occupancy_runner.hpp +++ b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_occupancy_runner.hpp @@ -74,23 +74,6 @@ class PromptOccupancyRunner : public ServiceRunner return request; } - - /** - * @brief generate a typed erased response - * - * this method is used in a callback passed to the trigger caller to get type erased result - * from the service the reponse can be passed by the caller or ignored - * - * The pattern needs to be implemented in the derived class - * - * @param wrapped_result - * @return tinyxml2::XMLElement* - */ - virtual tinyxml2::XMLElement* - generate_response(const typename prompt_msgs::srv::Prompt::Response::SharedPtr& result) const override - { - return nullptr; - } }; } // namespace capabilities2_runner diff --git a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_request_runner.hpp b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_request_runner.hpp index c9c41c2..7e17207 100644 --- a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_request_runner.hpp +++ b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_request_runner.hpp @@ -56,13 +56,17 @@ class PromptPlanRequestRunner : public ServiceRunner if (!replan) { - request.prompt.prompt = "Build a xml plan based on the availbale capabilities to acheive mentioned task. Return only the xml plan without explanations or comments"; + request.prompt.prompt = "Build a xml plan based on the availbale capabilities to acheive mentioned task. Return " + "only the xml plan without explanations or comments"; } else { tinyxml2::XMLElement* failedElements = parameters->FirstChildElement("FailedElements"); - request.prompt.prompt = "Rebuild the xml plan based on the availbale capabilities to acheive mentioned task. Just give the xml plan without explanations or comments. These XML elements had incompatibilities. " + std::string(failedElements->GetText()) + "Recorrect them as well"; + request.prompt.prompt = "Rebuild the xml plan based on the availbale capabilities to acheive mentioned task. " + "Just give the xml plan without explanations or comments. These XML elements had " + "incompatibilities. " + + std::string(failedElements->GetText()) + "Recorrect them as well"; } prompt_msgs::msg::ModelOption modelOption1; @@ -81,25 +85,6 @@ class PromptPlanRequestRunner : public ServiceRunner return request; } - /** - * @brief generate a typed erased response - * - * this method is used in a callback passed to the trigger caller to get type erased result - * from the service the reponse can be passed by the caller or ignored - * - * The pattern needs to be implemented in the derived class - * - * @param wrapped_result - * @return tinyxml2::XMLElement* - */ - virtual tinyxml2::XMLElement* - generate_response(const prompt_msgs::srv::Prompt::Response::SharedPtr& result) const override - { - document_string = result->response.response; - - return nullptr; - } - /** * @brief Update on_success event parameters with new data if avaible. * @@ -111,21 +96,22 @@ class PromptPlanRequestRunner : public ServiceRunner * @param parameters pointer to the XMLElement containing parameters * @return pointer to the XMLElement containing updated parameters */ - virtual tinyxml2::XMLElement* update_on_success(tinyxml2::XMLElement* parameters) + virtual std::string update_on_success(std::string& parameters) { + tinyxml2::XMLElement* element = convert_to_xml(parameters); + + std::string document_string = response_->response.response; + // Create the plan element as a child of the existing parameters element - tinyxml2::XMLElement* textElement = parameters->GetDocument()->NewElement("ReceievdPlan"); - parameters->InsertEndChild(textElement); + tinyxml2::XMLElement* textElement = element->GetDocument()->NewElement("ReceievdPlan"); + element->InsertEndChild(textElement); textElement->SetText(document_string.c_str()); // Return the updated parameters element with Pose added - return parameters; - }; + std::string result = convert_to_string(element); - /** - * Document holder for received xml plan - */ - mutable std::string document_string; + return result; + }; }; } // namespace capabilities2_runner diff --git a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_response_runner.hpp b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_response_runner.hpp index 48e4c56..49557d3 100644 --- a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_response_runner.hpp +++ b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_response_runner.hpp @@ -60,18 +60,6 @@ class PromptPlanResponseRunner : public ActionRunnerSetAttribute("replan", true); + tinyxml2::XMLElement* element = convert_to_xml(parameters); + + element->SetAttribute("replan", true); // Create the failed elements element as a child of the existing parameters element - tinyxml2::XMLElement* failedElements = parameters->GetDocument()->NewElement("FailedElements"); - parameters->InsertEndChild(failedElements); + tinyxml2::XMLElement* failedElements = element->GetDocument()->NewElement("FailedElements"); + element->InsertEndChild(failedElements); std::string failedElementsString = ""; @@ -98,7 +88,9 @@ class PromptPlanResponseRunner : public ActionRunnerSetText(failedElementsString.c_str()); - return parameters; + std::string result = convert_to_string(element); + + return result; }; }; diff --git a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_pose_runner.hpp b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_pose_runner.hpp index 09ba97b..1205541 100644 --- a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_pose_runner.hpp +++ b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_pose_runner.hpp @@ -74,23 +74,6 @@ class PromptPoseRunner : public ServiceRunner return request; } - - /** - * @brief generate a typed erased response - * - * this method is used in a callback passed to the trigger caller to get type erased result - * from the service the reponse can be passed by the caller or ignored - * - * The pattern needs to be implemented in the derived class - * - * @param wrapped_result - * @return tinyxml2::XMLElement* - */ - virtual tinyxml2::XMLElement* - generate_response(const typename prompt_msgs::srv::Prompt::Response::SharedPtr& result) const override - { - return nullptr; - } }; } // namespace capabilities2_runner diff --git a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_text_runner.hpp b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_text_runner.hpp index 13713d1..0058a85 100644 --- a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_text_runner.hpp +++ b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_text_runner.hpp @@ -74,23 +74,6 @@ class PromptTextRunner : public ServiceRunner return request; } - - /** - * @brief generate a typed erased response - * - * this method is used in a callback passed to the trigger caller to get type erased result - * from the service the reponse can be passed by the caller or ignored - * - * The pattern needs to be implemented in the derived class - * - * @param wrapped_result - * @return tinyxml2::XMLElement* - */ - virtual tinyxml2::XMLElement* - generate_response(const typename prompt_msgs::srv::Prompt::Response::SharedPtr& result) const override - { - return nullptr; - } }; } // namespace capabilities2_runner diff --git a/capabilities2_server/include/capabilities2_server/capabilities_api.hpp b/capabilities2_server/include/capabilities2_server/capabilities_api.hpp index 45f2bc0..55acc3b 100644 --- a/capabilities2_server/include/capabilities2_server/capabilities_api.hpp +++ b/capabilities2_server/include/capabilities2_server/capabilities_api.hpp @@ -280,7 +280,7 @@ class CapabilitiesAPI on_failure_parameters, on_success_capability, on_success_parameters, on_stopped_capability, on_stopped_parameters); - RCLCPP_WARN(node_logging_interface_ptr_->get_logger(), "successfully set triggers"); + RCLCPP_INFO(node_logging_interface_ptr_->get_logger(), "successfully set triggers"); } catch (const capabilities2_runner::runner_exception& e) { diff --git a/capabilities2_server/include/capabilities2_server/runner_cache.hpp b/capabilities2_server/include/capabilities2_server/runner_cache.hpp index 1ebd874..b702409 100644 --- a/capabilities2_server/include/capabilities2_server/runner_cache.hpp +++ b/capabilities2_server/include/capabilities2_server/runner_cache.hpp @@ -98,17 +98,15 @@ class RunnerCache */ void trigger_runner(const std::string& capability, const std::string& parameters) { - tinyxml2::XMLDocument doc; - doc.Parse(parameters.c_str()); - tinyxml2::XMLElement* xml_parameters = doc.FirstChildElement(); - // is the runner in the cache if (running(capability)) { - runner_cache_[capability]->trigger(xml_parameters); + runner_cache_[capability]->trigger(parameters); } else { + RCLCPP_ERROR(node_logging_interface_ptr_->get_logger(), "Runner not found for capability: %s", + capability.c_str()); throw capabilities2_runner::runner_exception("capability runner not found: " + capability); } } @@ -135,98 +133,25 @@ class RunnerCache { capabilities2_runner::event_opts event_options; - if (on_started_capability != "") - { - event_options.on_started = [this, &on_started_capability, &capability](tinyxml2::XMLElement* parameters) { - RCLCPP_INFO(node_logging_interface_ptr_->get_logger(), "%s is triggering %s on_start", capability.c_str(), - on_started_capability.c_str()); - runner_cache_[on_started_capability]->trigger(parameters); - }; - - tinyxml2::XMLDocument doc; - doc.Parse(on_started_parameters.c_str()); - event_options.on_started_param = doc.FirstChildElement(); - } - else - { - event_options.on_started = [this, &capability](tinyxml2::XMLElement* parameters) { - RCLCPP_INFO(node_logging_interface_ptr_->get_logger(), "%s does not implement on_start event", - capability.c_str()); - }; - - event_options.on_started_param = nullptr; - } - - if (on_failure_capability != "") - { - event_options.on_failure = [this, &on_failure_capability, &capability](tinyxml2::XMLElement* parameters) { - RCLCPP_INFO(node_logging_interface_ptr_->get_logger(), "%s is triggering %s on_failure", capability.c_str(), - on_failure_capability.c_str()); - runner_cache_[on_failure_capability]->trigger(parameters); - }; - - tinyxml2::XMLDocument doc; - doc.Parse(on_failure_parameters.c_str()); - event_options.on_failure_param = doc.FirstChildElement(); - } - else - { - event_options.on_failure = [this, &capability](tinyxml2::XMLElement* parameters) { - RCLCPP_INFO(node_logging_interface_ptr_->get_logger(), "%s does not implement on_failure event", - capability.c_str()); - } ; - - event_options.on_failure_param = nullptr; - } - - if (on_success_capability != "") - { - event_options.on_success = [this, &on_success_capability, &capability](tinyxml2::XMLElement* parameters) { - runner_cache_[on_success_capability]->trigger(parameters); - RCLCPP_INFO(node_logging_interface_ptr_->get_logger(), "%s is triggering %s on_success", capability.c_str(), - on_success_capability.c_str()); - }; - - tinyxml2::XMLDocument doc; - doc.Parse(on_success_parameters.c_str()); - event_options.on_success_param = doc.FirstChildElement(); - } - else - { - event_options.on_success = [this, &capability](tinyxml2::XMLElement* parameters) { - RCLCPP_INFO(node_logging_interface_ptr_->get_logger(), "%s does not implement on_success event", - capability.c_str()); - } ; - - event_options.on_success_param = nullptr; - } - - if (on_stopped_capability != "") - { - event_options.on_stopped = [this, &on_stopped_capability, &capability](tinyxml2::XMLElement* parameters) { - runner_cache_[on_stopped_capability]->trigger(parameters); - RCLCPP_INFO(node_logging_interface_ptr_->get_logger(), "%s is triggering %s on_stopped", capability.c_str(), - on_stopped_capability.c_str()); - }; - - tinyxml2::XMLDocument doc; - doc.Parse(on_stopped_parameters.c_str()); - event_options.on_stopped_param = doc.FirstChildElement(); - } - else - { - event_options.on_stopped = [this, &capability](tinyxml2::XMLElement* parameters) { - RCLCPP_INFO(node_logging_interface_ptr_->get_logger(), "%s does not implement on_stopped event", - capability.c_str()); - } ; - - event_options.on_stopped_param = nullptr; - } - - int event_count = runner_cache_[capability]->attach_events(event_options); - - std::string event_name = capability + "_" + std::to_string(event_count); - event_cache_[event_name] = event_options; + event_options.on_started = on_started_capability; + event_options.on_failure = on_failure_capability; + event_options.on_success = on_success_capability; + event_options.on_stopped = on_stopped_capability; + + event_options.on_started_param = on_started_parameters; + event_options.on_failure_param = on_failure_parameters; + event_options.on_success_param = on_success_parameters; + event_options.on_stopped_param = on_stopped_parameters; + + int event_count = runner_cache_[capability]->attach_events( + event_options, std::bind(&capabilities2_server::RunnerCache::trigger_runner, this, std::placeholders::_1, + std::placeholders::_2)); + + RCLCPP_INFO(node_logging_interface_ptr_->get_logger(), + "Configured triggers for capability %s: \n\tStarted: %s \n\tFailure: %s \n\tSuccess: %s \n\tStopped: " + "%s\n", + capability.c_str(), on_started_capability.c_str(), on_failure_capability.c_str(), + on_success_capability.c_str(), on_stopped_capability.c_str()); } /** @@ -336,9 +261,6 @@ class RunnerCache // capability / provider specs -> runner std::map> runner_cache_; - // map events to capabilities - std::map event_cache_; - // runner plugin loader pluginlib::ClassLoader runner_loader_; diff --git a/capabilities2_server/launch/server.launch.py b/capabilities2_server/launch/server.launch.py index 4f70a3e..a7b4fa8 100644 --- a/capabilities2_server/launch/server.launch.py +++ b/capabilities2_server/launch/server.launch.py @@ -24,7 +24,7 @@ def generate_launch_description(): name='capabilities2_container', namespace='', package='rclcpp_components', - executable='component_container', + executable='component_container_mt', # prefix=['xterm -e gdb -ex run --args'], # Add GDB debugging prefix arguments=['--ros-args', '--log-level', 'info'], composable_node_descriptions=[ From 7b197a0030c7a5165a46987286b189644e97026a Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Tue, 7 Jan 2025 14:49:36 +1100 Subject: [PATCH 064/133] updated plans --- capabilities2_fabric/plans/navigation_1.xml | 2 +- capabilities2_fabric/plans/navigation_2.xml | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/capabilities2_fabric/plans/navigation_1.xml b/capabilities2_fabric/plans/navigation_1.xml index 3b6de79..4dbae38 100644 --- a/capabilities2_fabric/plans/navigation_1.xml +++ b/capabilities2_fabric/plans/navigation_1.xml @@ -1,6 +1,6 @@ - + \ No newline at end of file diff --git a/capabilities2_fabric/plans/navigation_2.xml b/capabilities2_fabric/plans/navigation_2.xml index a2569f7..4b0560a 100644 --- a/capabilities2_fabric/plans/navigation_2.xml +++ b/capabilities2_fabric/plans/navigation_2.xml @@ -1,7 +1,7 @@ - - + + \ No newline at end of file From ebca92d94b4de62463b80a87afd7001045f2043e Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Tue, 7 Jan 2025 15:04:30 +1100 Subject: [PATCH 065/133] minor fixes --- .../capabilities_fabric.hpp | 2 +- .../capabilities2_runner/action_runner.hpp | 43 +++++++++++++++---- 2 files changed, 35 insertions(+), 10 deletions(-) diff --git a/capabilities2_fabric/include/capabilities2_executor/capabilities_fabric.hpp b/capabilities2_fabric/include/capabilities2_executor/capabilities_fabric.hpp index ef5bb30..0675617 100644 --- a/capabilities2_fabric/include/capabilities2_executor/capabilities_fabric.hpp +++ b/capabilities2_fabric/include/capabilities2_executor/capabilities_fabric.hpp @@ -588,7 +588,7 @@ class CapabilitiesFabric : public rclcpp::Node auto response = future.get(); bond_id = response->bond_id; - feedback->progress = "Received the bond id : " + bond_id.c_str() ; + feedback->progress = "Received the bond id : " + bond_id; goal_handle->publish_feedback(feedback); RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); diff --git a/capabilities2_runner/include/capabilities2_runner/action_runner.hpp b/capabilities2_runner/include/capabilities2_runner/action_runner.hpp index f438e52..3d6bd35 100644 --- a/capabilities2_runner/include/capabilities2_runner/action_runner.hpp +++ b/capabilities2_runner/include/capabilities2_runner/action_runner.hpp @@ -47,15 +47,15 @@ class ActionRunner : public RunnerBase action_client_ = rclcpp_action::create_client(node_, action_name); // wait for action server - RCLCPP_INFO(node_->get_logger(), "%s waiting for action: %s", run_config_.interface.c_str(), action_name.c_str()); + RCLCPP_INFO(node_->get_logger(), "[%s] waiting for action: %s", run_config_.interface.c_str(), action_name.c_str()); if (!action_client_->wait_for_action_server(std::chrono::seconds(1000))) { - RCLCPP_ERROR(node_->get_logger(), "%s failed to connect to action: %s", run_config_.interface.c_str(), + 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"); } - RCLCPP_INFO(node_->get_logger(), "%s connected with action: %s", run_config_.interface.c_str(), + RCLCPP_INFO(node_->get_logger(), "[%s] connected with action: %s", run_config_.interface.c_str(), action_name.c_str()); } @@ -92,6 +92,10 @@ class ActionRunner : public RunnerBase } // Trigger on_stopped event if defined + + RCLCPP_INFO(node_->get_logger(), "[%s] on_stopped event available. Triggering", + run_config_.interface.c_str()); + events[execute_id].on_stopped(update_on_stopped(events[execute_id].on_stopped_param)); }); @@ -137,15 +141,21 @@ class ActionRunner : public RunnerBase // generate a goal from parameters if provided typename ActionT::Goal goal_msg = generate_goal(parameters); + RCLCPP_INFO(node_->get_logger(), "[%s] goal generated.", run_config_.interface.c_str()); + // send goal options // goal response callback send_goal_options_.goal_response_callback = [this](const typename rclcpp_action::ClientGoalHandle::SharedPtr& goal_handle) { if (goal_handle) + { + RCLCPP_INFO(node_->get_logger(), "[%s] on_started event available. Triggering", + run_config_.interface.c_str()); events[execute_id].on_started(update_on_started(events[execute_id].on_started_param)); + } else { - RCLCPP_ERROR(node_->get_logger(), "Goal was rejected by server"); + RCLCPP_ERROR(node_->get_logger(), "[%s] goal rejected", run_config_.interface.c_str()); } // store goal handle to be used with stop funtion @@ -155,10 +165,21 @@ class ActionRunner : public RunnerBase // result callback send_goal_options_.result_callback = [this](const typename rclcpp_action::ClientGoalHandle::WrappedResult& wrapped_result) { - if (wrapped_result.code == rclcpp_action::ResultCode::SUCCEEDED) - events[execute_id].on_success(update_on_success(events[execute_id].on_success_param)); // Trigger on_success event if defined - else - events[execute_id].on_failure(update_on_failure(events[execute_id].on_failure_param)); // Trigger on_failure event if defined + if (wrapped_result.code == rclcpp_action::ResultCode::SUCCEEDED) + { + // Trigger on_success event if defined + RCLCPP_INFO(node_->get_logger(), "[%s] on_success event available. Triggering", + run_config_.interface.c_str()); + events[execute_id].on_success(update_on_success(events[execute_id].on_success_param)); + } + else + { + // Trigger on_failure event if defined + RCLCPP_INFO(node_->get_logger(), "[%s] on_failure event available. Triggering", + run_config_.interface.c_str()); + + events[execute_id].on_failure(update_on_failure(events[execute_id].on_failure_param)); + } result_ = wrapped_result.result; }; @@ -166,6 +187,8 @@ class ActionRunner : public RunnerBase // trigger the action client with goal auto goal_handle_future = action_client_->async_send_goal(goal_msg, send_goal_options_); + RCLCPP_INFO(node_->get_logger(), "[%s] goal sent. Waiting for acceptance", run_config_.interface.c_str()); + // create a function to call for the result. the future will be returned to the caller and the caller // can provide a conversion function to handle the result @@ -173,9 +196,11 @@ class ActionRunner : public RunnerBase goal_handle_future](tinyxml2::XMLElement* result) { auto goal_handle = goal_handle_future.get(); + RCLCPP_INFO(node_->get_logger(), "[%s] goal accepted. Waiting for result", run_config_.interface.c_str()); + if (!goal_handle) { - RCLCPP_ERROR(node_->get_logger(), "Failed to send goal"); + RCLCPP_INFO(node_->get_logger(), "[%s] goal rejected", run_config_.interface.c_str()); return; } From 225db11af636a99f27e0dd13171307b7e2d666df Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Thu, 9 Jan 2025 03:28:18 +1100 Subject: [PATCH 066/133] minor fixes --- .../docs/waypoint_runner_ex1.md | 2 +- .../docs/waypoint_runner_ex2.md | 2 +- .../capabilities2_runner/action_runner.hpp | 208 +++++++++++------- .../capabilities2_runner/launch_runner.hpp | 2 +- .../notrigger_action_runner.hpp | 2 +- .../capabilities2_runner/runner_base.hpp | 6 +- .../capabilities2_runner/service_runner.hpp | 2 +- .../capabilities2_runner/topic_runner.hpp | 2 +- capabilities2_runner/src/standard_runners.cpp | 2 +- 9 files changed, 142 insertions(+), 86 deletions(-) diff --git a/capabilities2_fabric/docs/waypoint_runner_ex1.md b/capabilities2_fabric/docs/waypoint_runner_ex1.md index 76ef73a..555c773 100644 --- a/capabilities2_fabric/docs/waypoint_runner_ex1.md +++ b/capabilities2_fabric/docs/waypoint_runner_ex1.md @@ -39,7 +39,7 @@ colcon build ```bash export TURTLEBOT3_MODEL=waffle -ros2 launch turtlebot3_gazebo empty_world.launch.py +ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py ``` ### Start the Navigation2 stack diff --git a/capabilities2_fabric/docs/waypoint_runner_ex2.md b/capabilities2_fabric/docs/waypoint_runner_ex2.md index 6cf251a..01121e9 100644 --- a/capabilities2_fabric/docs/waypoint_runner_ex2.md +++ b/capabilities2_fabric/docs/waypoint_runner_ex2.md @@ -39,7 +39,7 @@ colcon build ```bash export TURTLEBOT3_MODEL=waffle -ros2 launch turtlebot3_gazebo turtlebot3_empty_world.launch.py +ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py ``` ### Start the Navigation2 stack diff --git a/capabilities2_runner/include/capabilities2_runner/action_runner.hpp b/capabilities2_runner/include/capabilities2_runner/action_runner.hpp index 54429ee..06e1135 100644 --- a/capabilities2_runner/include/capabilities2_runner/action_runner.hpp +++ b/capabilities2_runner/include/capabilities2_runner/action_runner.hpp @@ -1,6 +1,7 @@ #pragma once #include +#include #include #include #include @@ -47,15 +48,15 @@ class ActionRunner : public RunnerBase action_client_ = rclcpp_action::create_client(node_, action_name); // wait for action server - RCLCPP_INFO(node_->get_logger(), "%s waiting for action: %s", run_config_.interface.c_str(), action_name.c_str()); + RCLCPP_INFO(node_->get_logger(), "[%s] waiting for action: %s", run_config_.interface.c_str(), action_name.c_str()); if (!action_client_->wait_for_action_server(std::chrono::seconds(1000))) { - RCLCPP_ERROR(node_->get_logger(), "%s failed to connect to action: %s", run_config_.interface.c_str(), + 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"); } - RCLCPP_INFO(node_->get_logger(), "%s connected with action: %s", run_config_.interface.c_str(), + RCLCPP_INFO(node_->get_logger(), "[%s] connected with action: %s", run_config_.interface.c_str(), action_name.c_str()); } @@ -82,28 +83,11 @@ class ActionRunner : public RunnerBase { try { - std::shared_future cancel_future = - action_client_->async_cancel_goal( - goal_handle_, [this](action_msgs::srv::CancelGoal_Response::SharedPtr response) { - if (response->return_code != action_msgs::srv::CancelGoal_Response::ERROR_NONE) - { - // throw runner_exception("failed to cancel runner"); - RCLCPP_WARN(node_->get_logger(), "Runner cancellation failed."); - } - - // Trigger on_stopped event if defined - if (events[execute_id].on_stopped != "") - { - RCLCPP_INFO(node_->get_logger(), "[%s] on_stopped event available. Triggering", - run_config_.interface.c_str()); - triggerFunction_(events[execute_id].on_stopped, - update_on_stopped(events[execute_id].on_stopped_param)); - } - }); + auto cancel_future = action_client_->async_cancel_goal( + goal_handle_, std::bind(&ActionRunner::cancellationCallback, this, std::placeholders::_1)); // wait for action to be stopped. hold the thread for 2 seconds to help keep callbacks in scope // BUG: the line below does not work in jazzy build, so a workaround is used - // rclcpp::spin_until_future_complete(node_->get_node_base_interface(), cancel_future, std::chrono::seconds(2)); auto timeout = std::chrono::steady_clock::now() + std::chrono::seconds(2); while (std::chrono::steady_clock::now() < timeout) { @@ -130,7 +114,7 @@ class ActionRunner : public RunnerBase * * @param parameters pointer to tinyxml2::XMLElement that contains parameters */ - virtual void triggerExecution() override + virtual void execution() override { execute_id += 1; @@ -139,80 +123,135 @@ class ActionRunner : public RunnerBase throw runner_exception("cannot trigger action without parameters"); // generate a goal from parameters if provided - typename ActionT::Goal goal_msg = generate_goal(parameters_); + goal_msg_ = generate_goal(parameters_); RCLCPP_INFO(node_->get_logger(), "[%s] goal generated.", run_config_.interface.c_str()); - // send goal options - // goal response callback - send_goal_options_.goal_response_callback = - [this](const typename rclcpp_action::ClientGoalHandle::SharedPtr& goal_handle) { - if (goal_handle) - { - // trigger the events related to on_started state - if (events[execute_id].on_started != "") - { - RCLCPP_INFO(node_->get_logger(), "[%s] on_started event available. Triggering", - run_config_.interface.c_str()); - triggerFunction_(events[execute_id].on_started, update_on_started(events[execute_id].on_started_param)); - } - } - else - { - RCLCPP_ERROR(node_->get_logger(), "[%s] goal rejected", run_config_.interface.c_str()); - } - - // 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) - { - // trigger the events related to on_success state - if (events[execute_id].on_success != "") - { - RCLCPP_INFO(node_->get_logger(), "[%s] on_success event available. Triggering", - run_config_.interface.c_str()); - triggerFunction_(events[execute_id].on_success, update_on_success(events[execute_id].on_success_param)); - } - } - else - { - // trigger the events related to on_failure state - if (events[execute_id].on_failure != "") - { - RCLCPP_INFO(node_->get_logger(), "[%s] on_failure event available. Triggering", - run_config_.interface.c_str()); - triggerFunction_(events[execute_id].on_failure, update_on_failure(events[execute_id].on_failure_param)); - } - } - - result_ = wrapped_result.result; - }; + // Synchronization tools + action_completed_ = false; // trigger the action client with goal - auto goal_handle_future = action_client_->async_send_goal(goal_msg, send_goal_options_); + goal_handle_future_ = action_client_->async_send_goal(goal_msg_); RCLCPP_INFO(node_->get_logger(), "[%s] goal sent. Waiting for acceptance", run_config_.interface.c_str()); // create a function to call for the result. the future will be returned to the caller and the caller // can provide a conversion function to handle the result - auto goal_handle = goal_handle_future.get(); + goal_handle_ = goal_handle_future_.get(); RCLCPP_INFO(node_->get_logger(), "[%s] goal accepted. Waiting for result", run_config_.interface.c_str()); - if (!goal_handle) + if (!goal_handle_) { RCLCPP_INFO(node_->get_logger(), "[%s] goal rejected", run_config_.interface.c_str()); return; } + + // Get the result asynchronously + result_future_ = action_client_->async_get_result(goal_handle_); + + while (true) + { + // Check if the cancel operation is complete + if (result_future_.wait_for(std::chrono::milliseconds(200)) == std::future_status::ready) + break; + + RCLCPP_INFO(node_->get_logger(), "[%s] waiting for result..", run_config_.interface.c_str()); + } + + RCLCPP_INFO(node_->get_logger(), "[%s] received result", run_config_.interface.c_str()); + + // Wait for result future + wrapped_result_ = result_future_.get(); + + // convert the result + if (wrapped_result_.code == rclcpp_action::ResultCode::SUCCEEDED) + { + RCLCPP_INFO(node_->get_logger(), "[%s] successfully completed", run_config_.interface.c_str()); + } + else + { + RCLCPP_INFO(node_->get_logger(), "[%s] failed to complete", run_config_.interface.c_str()); + } + + auto timeout = std::chrono::steady_clock::now() + std::chrono::seconds(20); + + // std::unique_lock lock(mutex_); + // cv_.wait(lock, [&action_completed] { return action_completed; }); } protected: + // virtual void goalResponseCallback(const typename rclcpp_action::ClientGoalHandle::SharedPtr& goal_handle) + // { + // if (goal_handle) + // { + // RCLCPP_INFO(node_->get_logger(), "[%s] goal accepted. Waiting for result", run_config_.interface.c_str()); + + // // trigger the events related to on_started state + // if (events[execute_id].on_started != "") + // { + // RCLCPP_INFO(node_->get_logger(), "[%s] on_started event available. Triggering", + // run_config_.interface.c_str()); triggerFunction_(events[execute_id].on_started, + // update_on_started(events[execute_id].on_started_param)); + // } + // } + // else + // { + // RCLCPP_ERROR(node_->get_logger(), "[%s] goal rejected", run_config_.interface.c_str()); + // } + + // // store goal handle to be used with stop funtion + // goal_handle_ = goal_handle; + // }; + + // virtual void resultCallback(const typename rclcpp_action::ClientGoalHandle::WrappedResult& wrapped_result) + // { + // std::lock_guard lock(mutex_); + + // if (wrapped_result.code == rclcpp_action::ResultCode::SUCCEEDED) + // { + // // trigger the events related to on_success state + // if (events[execute_id].on_success != "") + // { + // RCLCPP_INFO(node_->get_logger(), "[%s] on_success event available. Triggering", + // run_config_.interface.c_str()); triggerFunction_(events[execute_id].on_success, + // update_on_success(events[execute_id].on_success_param)); + // } + // } + // else + // { + // // trigger the events related to on_failure state + // if (events[execute_id].on_failure != "") + // { + // RCLCPP_INFO(node_->get_logger(), "[%s] on_failure event available. Triggering", + // run_config_.interface.c_str()); triggerFunction_(events[execute_id].on_failure, + // update_on_failure(events[execute_id].on_failure_param)); + // } + // } + + // result_ = wrapped_result.result; + + // action_completed_ = true; // Mark action as completed + // cv_.notify_one(); // Notify the waiting thread + // }; + + virtual void cancellationCallback(action_msgs::srv::CancelGoal_Response::SharedPtr response) + { + if (response->return_code != action_msgs::srv::CancelGoal_Response::ERROR_NONE) + { + // throw runner_exception("failed to cancel runner"); + RCLCPP_WARN(node_->get_logger(), "Runner cancellation failed."); + } + + // Trigger on_stopped event if defined + if (events[execute_id].on_stopped != "") + { + RCLCPP_INFO(node_->get_logger(), "[%s] on_stopped event available. Triggering", run_config_.interface.c_str()); + triggerFunction_(events[execute_id].on_stopped, update_on_stopped(events[execute_id].on_stopped_param)); + } + } + /** * @brief Generate a goal from parameters * @@ -236,8 +275,25 @@ class ActionRunner : public RunnerBase /** goal handle parameter to capture goal response from goal_response_callback */ typename rclcpp_action::ClientGoalHandle::SharedPtr goal_handle_; + /** Wrapped Result */ + typename rclcpp_action::ClientGoalHandle::WrappedResult wrapped_result_; + /** Result */ typename ActionT::Result::SharedPtr result_; + + /** Goal message */ + typename ActionT::Goal goal_msg_; + + /** Goal Handle Future message */ + std::shared_future::SharedPtr> goal_handle_future_; + + /** Result Future*/ + std::shared_future::WrappedResult> result_future_; + + // Synchronization tools + std::mutex mutex_; + std::condition_variable cv_; + bool action_completed_; }; } // namespace capabilities2_runner diff --git a/capabilities2_runner/include/capabilities2_runner/launch_runner.hpp b/capabilities2_runner/include/capabilities2_runner/launch_runner.hpp index 22ff606..dcc8781 100644 --- a/capabilities2_runner/include/capabilities2_runner/launch_runner.hpp +++ b/capabilities2_runner/include/capabilities2_runner/launch_runner.hpp @@ -129,7 +129,7 @@ class LaunchRunner : public RunnerBase protected: // throw on triggerExecution function - void triggerExecution() override + void execution() override { throw runner_exception("no triggerExecution() this is a no-trigger action runner"); } diff --git a/capabilities2_runner/include/capabilities2_runner/notrigger_action_runner.hpp b/capabilities2_runner/include/capabilities2_runner/notrigger_action_runner.hpp index de9c10b..1287105 100644 --- a/capabilities2_runner/include/capabilities2_runner/notrigger_action_runner.hpp +++ b/capabilities2_runner/include/capabilities2_runner/notrigger_action_runner.hpp @@ -26,7 +26,7 @@ class NoTriggerActionRunner : public ActionRunner protected: // throw on triggerExecution function - void triggerExecution() override + void execution() override { throw runner_exception("no triggerExecution() this is a no-trigger action runner"); } diff --git a/capabilities2_runner/include/capabilities2_runner/runner_base.hpp b/capabilities2_runner/include/capabilities2_runner/runner_base.hpp index 5a0db5e..e48cea1 100644 --- a/capabilities2_runner/include/capabilities2_runner/runner_base.hpp +++ b/capabilities2_runner/include/capabilities2_runner/runner_base.hpp @@ -124,7 +124,7 @@ class RunnerBase { parameters_ = convert_to_xml(parameters); - executionThread = std::thread(&RunnerBase::triggerExecution, this); + executionThread = std::thread(&RunnerBase::execution, this); } /** @@ -152,7 +152,7 @@ class RunnerBase int attach_events(capabilities2_runner::event_opts& event_option, std::function triggerFunction) { - RCLCPP_INFO(node_->get_logger(), "%s accepted event options with ID : %d ", run_config_.interface.c_str(), + RCLCPP_INFO(node_->get_logger(), "[%s] accepted event options with ID : %d ", run_config_.interface.c_str(), insert_id); triggerFunction_ = triggerFunction; @@ -212,7 +212,7 @@ class RunnerBase * @param parameters pointer to tinyxml2::XMLElement that contains parameters * */ - virtual void triggerExecution() = 0; + virtual void execution() = 0; /** * @brief Update on_started event parameters with new data if avaible. diff --git a/capabilities2_runner/include/capabilities2_runner/service_runner.hpp b/capabilities2_runner/include/capabilities2_runner/service_runner.hpp index 5058784..498d18a 100644 --- a/capabilities2_runner/include/capabilities2_runner/service_runner.hpp +++ b/capabilities2_runner/include/capabilities2_runner/service_runner.hpp @@ -58,7 +58,7 @@ class ServiceRunner : public RunnerBase * * @param parameters pointer to tinyxml2::XMLElement that contains parameters */ - virtual void triggerExecution() override + virtual void execution() override { execute_id += 1; diff --git a/capabilities2_runner/include/capabilities2_runner/topic_runner.hpp b/capabilities2_runner/include/capabilities2_runner/topic_runner.hpp index 64c26c3..26f5840 100644 --- a/capabilities2_runner/include/capabilities2_runner/topic_runner.hpp +++ b/capabilities2_runner/include/capabilities2_runner/topic_runner.hpp @@ -50,7 +50,7 @@ class TopicRunner : public RunnerBase * * @param parameters pointer to tinyxml2::XMLElement that contains parameters */ - virtual void triggerExecution() override + virtual void execution() override { execute_id += 1; diff --git a/capabilities2_runner/src/standard_runners.cpp b/capabilities2_runner/src/standard_runners.cpp index c01054d..459aa42 100644 --- a/capabilities2_runner/src/standard_runners.cpp +++ b/capabilities2_runner/src/standard_runners.cpp @@ -33,7 +33,7 @@ class DummyRunner : public RunnerBase protected: // throw on triggerExecution function - void triggerExecution() override + void execution() override { RCLCPP_INFO(node_->get_logger(), "Dummy runner does not have triggerExecution()"); } From 5e97907c9abb2f2d166cfa051c5e3ed9afe0bb32 Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Thu, 9 Jan 2025 16:04:13 +1100 Subject: [PATCH 067/133] added succeed action server component to fabric --- .../capabilities_fabric.hpp | 39 ++++++++++++------- .../capabilities_file_parser.hpp | 32 ++++++--------- 2 files changed, 37 insertions(+), 34 deletions(-) diff --git a/capabilities2_fabric/include/capabilities2_executor/capabilities_fabric.hpp b/capabilities2_fabric/include/capabilities2_executor/capabilities_fabric.hpp index a1396a7..e6ff0de 100644 --- a/capabilities2_fabric/include/capabilities2_executor/capabilities_fabric.hpp +++ b/capabilities2_fabric/include/capabilities2_executor/capabilities_fabric.hpp @@ -164,7 +164,7 @@ class CapabilitiesFabric : public rclcpp::Node // check if the file parsing failed if (xml_status != tinyxml2::XMLError::XML_SUCCESS) { - RCLCPP_INFO(this->get_logger(), "Parsing the plan from goal message failed"); + RCLCPP_ERROR(this->get_logger(), "Parsing the plan from goal message failed"); return rclcpp_action::GoalResponse::REJECT; } @@ -181,7 +181,7 @@ class CapabilitiesFabric : public rclcpp::Node */ rclcpp_action::CancelResponse handle_cancel(const std::shared_ptr goal_handle) { - RCLCPP_INFO(this->get_logger(), "Received the request to cancel the plan"); + RCLCPP_ERROR(this->get_logger(), "Received the request to cancel the plan"); (void)goal_handle; return rclcpp_action::CancelResponse::ACCEPT; @@ -202,7 +202,11 @@ class CapabilitiesFabric : public rclcpp::Node */ void execution(const std::shared_ptr goal_handle) { - RCLCPP_INFO(this->get_logger(), "Execution started"); + auto feedback = std::make_shared(); + + feedback->progress = "Execution started"; + goal_handle->publish_feedback(feedback); + RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); expected_providers_ = 0; completed_providers_ = 0; @@ -228,8 +232,7 @@ class CapabilitiesFabric : public rclcpp::Node feedback->progress = "Requesting Interface information"; goal_handle->publish_feedback(feedback); - - RCLCPP_INFO(this->get_logger(), "Requesting Interface information"); + RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); auto request_interface = std::make_shared(); @@ -240,18 +243,20 @@ class CapabilitiesFabric : public rclcpp::Node if (!future.valid()) { - RCLCPP_ERROR(this->get_logger(), "Failed to get Interface information"); - result->success = false; result->message = "Failed to get Interface information"; goal_handle->abort(result); + RCLCPP_ERROR(this->get_logger(), result->message.c_str()); - RCLCPP_INFO(this->get_logger(), "Server Execution Cancelled"); + RCLCPP_ERROR(this->get_logger(), "Server Execution Cancelled"); return; } auto response = future.get(); - RCLCPP_INFO(this->get_logger(), "Received Interface information"); + + feedback->progress = "Received Interface information"; + goal_handle->publish_feedback(feedback); + RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); expected_interfaces_ = response->interfaces.size(); @@ -274,7 +279,6 @@ class CapabilitiesFabric : public rclcpp::Node RCLCPP_INFO(this->get_logger(), ""); feedback->progress = "Requesting semantic interfaces for " + requested_interface; goal_handle->publish_feedback(feedback); - RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); auto request_semantic = std::make_shared(); @@ -290,7 +294,7 @@ class CapabilitiesFabric : public rclcpp::Node result->message = "Failed to get Semantic Interface information"; goal_handle->abort(result); - RCLCPP_INFO(this->get_logger(), "Failed to get Semantic Interface information. Server Execution Cancelled"); + RCLCPP_ERROR(this->get_logger(), "Failed to get Semantic Interface information. Server Execution Cancelled"); return; } @@ -320,7 +324,6 @@ class CapabilitiesFabric : public rclcpp::Node feedback->progress = std::to_string(completed_interfaces_) + "/" + std::to_string(expected_interfaces_) + " : Received none for " + requested_interface + ". So added " + requested_interface; goal_handle->publish_feedback(feedback); - RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); } @@ -331,8 +334,10 @@ class CapabilitiesFabric : public rclcpp::Node } else { + feedback->progress = "Received all requested Interface information"; + goal_handle->publish_feedback(feedback); RCLCPP_INFO(this->get_logger(), ""); - RCLCPP_INFO(this->get_logger(), "Received all requested Interface information"); + RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); expected_providers_ = interface_list.size(); @@ -588,7 +593,7 @@ class CapabilitiesFabric : public rclcpp::Node auto response = future.get(); bond_id = response->bond_id; - feedback->progress = "Received the bond id : " + bond_id ; + feedback->progress = "Received the bond id : " + bond_id; goal_handle->publish_feedback(feedback); RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); @@ -888,6 +893,12 @@ class CapabilitiesFabric : public rclcpp::Node feedback->progress = "Successfully triggered capability " + connection_map[0].source.runner; goal_handle->publish_feedback(feedback); RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); + + auto result = std::make_shared(); + result->success = true; + result->message = "Successfully launched capabilities2 fabric"; + goal_handle->succeed(result); + RCLCPP_INFO(this->get_logger(), result->message.c_str()); }); } diff --git a/capabilities2_fabric/include/capabilities2_executor/capabilities_file_parser.hpp b/capabilities2_fabric/include/capabilities2_executor/capabilities_file_parser.hpp index c576712..8ca636a 100644 --- a/capabilities2_fabric/include/capabilities2_executor/capabilities_file_parser.hpp +++ b/capabilities2_fabric/include/capabilities2_executor/capabilities_file_parser.hpp @@ -38,14 +38,20 @@ class CapabilitiesFileParser : public rclcpp::Node // Create the action client for capabilities_fabric after the node is fully constructed this->client_ptr_ = rclcpp_action::create_client(shared_from_this(), "/capabilities_fabric"); - // Set up a timer to call send_goal after a short delay - this->timer_ = this->create_wall_timer(std::chrono::milliseconds(500), std::bind(&CapabilitiesFileParser::send_goal, this)); + if (!this->client_ptr_->wait_for_action_server(std::chrono::seconds(5))) + { + RCLCPP_ERROR(this->get_logger(), "Action server not available after waiting"); + rclcpp::shutdown(); + return; + } + + RCLCPP_INFO(this->get_logger(), "Sucessfully connected to the capabilities_fabric action server"); + + send_goal(); } void send_goal() { - this->timer_->cancel(); - // try to load the file tinyxml2::XMLError xml_status = document.LoadFile(plan_file_path.c_str()); @@ -58,20 +64,11 @@ class CapabilitiesFileParser : public rclcpp::Node RCLCPP_INFO(this->get_logger(), "Plan loaded from : %s", plan_file_path.c_str()); - if (!this->client_ptr_->wait_for_action_server(std::chrono::seconds(5))) - { - RCLCPP_ERROR(this->get_logger(), "Action server not available after waiting"); - rclcpp::shutdown(); - return; - } - - RCLCPP_INFO(this->get_logger(), "Sucessfully connected to the capabilities_fabric action server"); - auto goal_msg = capabilities2_msgs::action::Plan::Goal(); capabilities2_xml_parser::convert_to_string(document, goal_msg.plan); - RCLCPP_INFO(this->get_logger(), "Following plan was read:\n\n%s ", goal_msg.plan.c_str()); + RCLCPP_INFO(this->get_logger(), "Following plan was loaded :\n\n%s ", goal_msg.plan.c_str()); RCLCPP_INFO(this->get_logger(), "Sending goal to the capabilities_fabric action server"); @@ -110,7 +107,7 @@ class CapabilitiesFileParser : public rclcpp::Node if (result.result->success) { - RCLCPP_ERROR(this->get_logger(), "Plan executed successfully"); + RCLCPP_INFO(this->get_logger(), "Plan executed successfully"); } else { @@ -124,8 +121,6 @@ class CapabilitiesFileParser : public rclcpp::Node RCLCPP_ERROR(this->get_logger(), "Failed Elements : %s", failed_element.c_str()); } } - - rclcpp::shutdown(); }; this->client_ptr_->async_send_goal(goal_msg, send_goal_options); @@ -140,7 +135,4 @@ class CapabilitiesFileParser : public rclcpp::Node /** action client */ rclcpp_action::Client::SharedPtr client_ptr_; - - /** action server */ - rclcpp::TimerBase::SharedPtr timer_; }; \ No newline at end of file From 330a7e5d9450e925320aee946806a3b47ff8a216 Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Fri, 10 Jan 2025 00:56:46 +1100 Subject: [PATCH 068/133] added feedback to information --- .../capabilities2_runner/action_runner.hpp | 159 ++++++++---------- .../listen_runner.hpp | 15 +- .../speak_runner.hpp | 13 ++ .../waypoint_runner.hpp | 15 ++ .../prompt_plan_response_runner.hpp | 13 ++ 5 files changed, 123 insertions(+), 92 deletions(-) diff --git a/capabilities2_runner/include/capabilities2_runner/action_runner.hpp b/capabilities2_runner/include/capabilities2_runner/action_runner.hpp index 06e1135..1b2e82b 100644 --- a/capabilities2_runner/include/capabilities2_runner/action_runner.hpp +++ b/capabilities2_runner/include/capabilities2_runner/action_runner.hpp @@ -124,117 +124,82 @@ class ActionRunner : public RunnerBase // generate a goal from parameters if provided goal_msg_ = generate_goal(parameters_); - RCLCPP_INFO(node_->get_logger(), "[%s] goal generated.", run_config_.interface.c_str()); - // Synchronization tools - action_completed_ = false; - // trigger the action client with goal - goal_handle_future_ = action_client_->async_send_goal(goal_msg_); - - RCLCPP_INFO(node_->get_logger(), "[%s] goal sent. Waiting for acceptance", run_config_.interface.c_str()); - - // create a function to call for the result. the future will be returned to the caller and the caller - // can provide a conversion function to handle the result + send_goal_options_.goal_response_callback = + std::bind(&ActionRunner::goalResponseCallback, this, std::placeholders::_1); + send_goal_options_.feedback_callback = + std::bind(&ActionRunner::feedbackCallback, this, std::placeholders::_1, std::placeholders::_2); + send_goal_options_.result_callback = std::bind(&ActionRunner::resultCallback, this, std::placeholders::_1); - goal_handle_ = goal_handle_future_.get(); + goal_handle_future_ = action_client_->async_send_goal(goal_msg_, send_goal_options_); - RCLCPP_INFO(node_->get_logger(), "[%s] goal accepted. Waiting for result", run_config_.interface.c_str()); + RCLCPP_INFO(node_->get_logger(), "[%s] goal sent. Waiting for acceptance", run_config_.interface.c_str()); + } - if (!goal_handle_) +protected: + virtual void goalResponseCallback(const typename rclcpp_action::ClientGoalHandle::SharedPtr& goal_handle) + { + if (goal_handle) { - RCLCPP_INFO(node_->get_logger(), "[%s] goal rejected", run_config_.interface.c_str()); - return; - } - - // Get the result asynchronously - result_future_ = action_client_->async_get_result(goal_handle_); + RCLCPP_INFO(node_->get_logger(), "[%s] goal accepted. Waiting for result", run_config_.interface.c_str()); - while (true) + // trigger the events related to on_started state + if (events[execute_id].on_started != "") + { + RCLCPP_INFO(node_->get_logger(), "[%s] on_started event available. Triggering", run_config_.interface.c_str()); + // triggerFunction_(events[execute_id].on_started, update_on_started(events[execute_id].on_started_param)); + } + } + else { - // Check if the cancel operation is complete - if (result_future_.wait_for(std::chrono::milliseconds(200)) == std::future_status::ready) - break; - - RCLCPP_INFO(node_->get_logger(), "[%s] waiting for result..", run_config_.interface.c_str()); + RCLCPP_ERROR(node_->get_logger(), "[%s] goal rejected", run_config_.interface.c_str()); } - RCLCPP_INFO(node_->get_logger(), "[%s] received result", run_config_.interface.c_str()); + // store goal handle to be used with stop funtion + goal_handle_ = goal_handle; + }; - // Wait for result future - wrapped_result_ = result_future_.get(); + virtual void resultCallback(const typename rclcpp_action::ClientGoalHandle::WrappedResult& wrapped_result) + { + RCLCPP_INFO(node_->get_logger(), "[%s] received result", run_config_.interface.c_str()); - // convert the result - if (wrapped_result_.code == rclcpp_action::ResultCode::SUCCEEDED) + if (wrapped_result.code == rclcpp_action::ResultCode::SUCCEEDED) { - RCLCPP_INFO(node_->get_logger(), "[%s] successfully completed", run_config_.interface.c_str()); + RCLCPP_INFO(node_->get_logger(), "[%s] action succeeded.", run_config_.interface.c_str()); + + // trigger the events related to on_success state + if (events[execute_id].on_success != "") + { + RCLCPP_INFO(node_->get_logger(), "[%s] on_success event available. Triggering", run_config_.interface.c_str()); + // triggerFunction_(events[execute_id].on_success, update_on_success(events[execute_id].on_success_param)); + } } else { - RCLCPP_INFO(node_->get_logger(), "[%s] failed to complete", run_config_.interface.c_str()); + RCLCPP_ERROR(node_->get_logger(), "[%s] action failed.", run_config_.interface.c_str()); + RCLCPP_ERROR(node_->get_logger(), "[%s] error code: %d", run_config_.interface.c_str(), + static_cast(wrapped_result.code)); + + // trigger the events related to on_failure state + if (events[execute_id].on_failure != "") + { + RCLCPP_INFO(node_->get_logger(), "[%s] on_failure event available. Triggering", run_config_.interface.c_str()); + // triggerFunction_(events[execute_id].on_failure, update_on_failure(events[execute_id].on_failure_param)); + } } - auto timeout = std::chrono::steady_clock::now() + std::chrono::seconds(20); + result_ = wrapped_result.result; + }; - // std::unique_lock lock(mutex_); - // cv_.wait(lock, [&action_completed] { return action_completed; }); - } + virtual void feedbackCallback(typename rclcpp_action::ClientGoalHandle::SharedPtr goal_handle, + const typename ActionT::Feedback::ConstSharedPtr feedback_msg) + { + std::string feedback = generate_feedback(feedback_msg); -protected: - // virtual void goalResponseCallback(const typename rclcpp_action::ClientGoalHandle::SharedPtr& goal_handle) - // { - // if (goal_handle) - // { - // RCLCPP_INFO(node_->get_logger(), "[%s] goal accepted. Waiting for result", run_config_.interface.c_str()); - - // // trigger the events related to on_started state - // if (events[execute_id].on_started != "") - // { - // RCLCPP_INFO(node_->get_logger(), "[%s] on_started event available. Triggering", - // run_config_.interface.c_str()); triggerFunction_(events[execute_id].on_started, - // update_on_started(events[execute_id].on_started_param)); - // } - // } - // else - // { - // RCLCPP_ERROR(node_->get_logger(), "[%s] goal rejected", run_config_.interface.c_str()); - // } - - // // store goal handle to be used with stop funtion - // goal_handle_ = goal_handle; - // }; - - // virtual void resultCallback(const typename rclcpp_action::ClientGoalHandle::WrappedResult& wrapped_result) - // { - // std::lock_guard lock(mutex_); - - // if (wrapped_result.code == rclcpp_action::ResultCode::SUCCEEDED) - // { - // // trigger the events related to on_success state - // if (events[execute_id].on_success != "") - // { - // RCLCPP_INFO(node_->get_logger(), "[%s] on_success event available. Triggering", - // run_config_.interface.c_str()); triggerFunction_(events[execute_id].on_success, - // update_on_success(events[execute_id].on_success_param)); - // } - // } - // else - // { - // // trigger the events related to on_failure state - // if (events[execute_id].on_failure != "") - // { - // RCLCPP_INFO(node_->get_logger(), "[%s] on_failure event available. Triggering", - // run_config_.interface.c_str()); triggerFunction_(events[execute_id].on_failure, - // update_on_failure(events[execute_id].on_failure_param)); - // } - // } - - // result_ = wrapped_result.result; - - // action_completed_ = true; // Mark action as completed - // cv_.notify_one(); // Notify the waiting thread - // }; + RCLCPP_INFO(node_->get_logger(), "[%s] received feedback: %s", run_config_.interface.c_str(), feedback.c_str()); + }; virtual void cancellationCallback(action_msgs::srv::CancelGoal_Response::SharedPtr response) { @@ -248,7 +213,7 @@ class ActionRunner : public RunnerBase if (events[execute_id].on_stopped != "") { RCLCPP_INFO(node_->get_logger(), "[%s] on_stopped event available. Triggering", run_config_.interface.c_str()); - triggerFunction_(events[execute_id].on_stopped, update_on_stopped(events[execute_id].on_stopped_param)); + // triggerFunction_(events[execute_id].on_stopped, update_on_stopped(events[execute_id].on_stopped_param)); } } @@ -265,6 +230,18 @@ class ActionRunner : public RunnerBase */ virtual typename ActionT::Goal generate_goal(tinyxml2::XMLElement* parameters) = 0; + /** + * @brief Generate a std::string from feedback message + * + * This function is used to convert feedback messages into generic strings + * + * A pattern needs to be implemented in the derived class + * + * @param parameters + * @return ActionT::Feedback the received feedback + */ + virtual std::string generate_feedback(const typename ActionT::Feedback::ConstSharedPtr msg) = 0; + /**< action client */ typename rclcpp_action::Client::SharedPtr action_client_; 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 59d2e8a..35ca8d2 100644 --- a/capabilities2_runner_audio/include/capabilities2_runner_audio/listen_runner.hpp +++ b/capabilities2_runner_audio/include/capabilities2_runner_audio/listen_runner.hpp @@ -55,6 +55,19 @@ class ListenerRunner : public ActionRunner return goal_msg; } + /** + * @brief This generate feedback function overrides the generate_feedback() function from ActionRunner() + * + * @param msg feedback message from the action server + * @return std::string of feedback information + */ + virtual std::string + generate_feedback(const typename hri_audio_msgs::action::SpeechToText::Feedback::ConstSharedPtr msg) override + { + std::string feedback = ""; + return feedback; + } + /** * @brief Update on_success event parameters with new data if avaible. * @@ -69,7 +82,7 @@ class ListenerRunner : public ActionRunner virtual std::string update_on_success(std::string& parameters) { tinyxml2::XMLElement* element = convert_to_xml(parameters); - + // Create the Pose element as a child of the existing parameters element tinyxml2::XMLElement* textElement = element->GetDocument()->NewElement("Text"); element->InsertEndChild(textElement); diff --git a/capabilities2_runner_audio/include/capabilities2_runner_audio/speak_runner.hpp b/capabilities2_runner_audio/include/capabilities2_runner_audio/speak_runner.hpp index 925d90f..7f55443 100644 --- a/capabilities2_runner_audio/include/capabilities2_runner_audio/speak_runner.hpp +++ b/capabilities2_runner_audio/include/capabilities2_runner_audio/speak_runner.hpp @@ -59,6 +59,19 @@ class SpeakerRunner : public ActionRunner return goal_msg; } + /** + * @brief This generate feedback function overrides the generate_feedback() function from ActionRunner() + * + * @param msg feedback message from the action server + * @return std::string of feedback information + */ + virtual std::string + generate_feedback(const typename hri_audio_msgs::action::TextToSpeech::Feedback::ConstSharedPtr msg) override + { + std::string feedback = ""; + return feedback; + } + }; } // namespace capabilities2_runner \ No newline at end of file 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 8d30bba..ebbf9b3 100644 --- a/capabilities2_runner_nav2/include/capabilities2_runner_nav2/waypoint_runner.hpp +++ b/capabilities2_runner_nav2/include/capabilities2_runner_nav2/waypoint_runner.hpp @@ -2,6 +2,7 @@ #include #include +#include #include #include #include @@ -68,6 +69,20 @@ class WayPointRunner : public ActionRunner return goal_msg; } + /** + * @brief This generate feedback function overrides the generate_feedback() function from ActionRunner() + * + * @param msg feedback message from the action server + * @return std::string of feedback information + */ + virtual std::string + generate_feedback(const typename nav2_msgs::action::NavigateToPose::Feedback::ConstSharedPtr msg) override + { + std::string feedback = "x: " + std::to_string(msg->current_pose.pose.position.x) + + " y: " + std::to_string(msg->current_pose.pose.position.y); + return feedback; + } + 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_prompt/include/capabilities2_runner_prompt/prompt_plan_response_runner.hpp b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_response_runner.hpp index 49557d3..67a5c31 100644 --- a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_response_runner.hpp +++ b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_response_runner.hpp @@ -60,6 +60,19 @@ class PromptPlanResponseRunner : public ActionRunnerprogress; + return feedback; + } + /** * @brief Update on_failure event parameters with new data if avaible. * From 2d4db8583f1a9d4ae83122f26dbdf7d2e3c498f8 Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Fri, 10 Jan 2025 16:31:32 +1100 Subject: [PATCH 069/133] added con-composable node --- capabilities2_fabric/CMakeLists.txt | 1 + capabilities2_fabric/package.xml | 1 + .../src/capabilities_fabric.cpp | 3 +- capabilities2_server/CMakeLists.txt | 42 +++++++++++++-- .../capabilities_server.hpp | 2 +- capabilities2_server/launch/server.launch.py | 24 +++------ .../launch/server_composed.launch.py | 51 +++++++++++++++++++ capabilities2_server/package.xml | 1 + ...erver.cpp => capabilities_server_comp.cpp} | 2 +- .../src/capabilities_server_node.cpp | 22 ++++++++ 10 files changed, 124 insertions(+), 25 deletions(-) create mode 100644 capabilities2_server/launch/server_composed.launch.py rename capabilities2_server/src/{capabilities_server.cpp => capabilities_server_comp.cpp} (94%) create mode 100644 capabilities2_server/src/capabilities_server_node.cpp diff --git a/capabilities2_fabric/CMakeLists.txt b/capabilities2_fabric/CMakeLists.txt index e7f275c..36639d6 100644 --- a/capabilities2_fabric/CMakeLists.txt +++ b/capabilities2_fabric/CMakeLists.txt @@ -15,6 +15,7 @@ find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_action REQUIRED) find_package(capabilities2_msgs REQUIRED) +find_package(backward_ros REQUIRED) # Locate the static version of tinyxml2 find_library(TINYXML2_LIB NAMES tinyxml2 PATHS /usr/local/lib NO_DEFAULT_PATH) diff --git a/capabilities2_fabric/package.xml b/capabilities2_fabric/package.xml index 25f55e8..43af93a 100644 --- a/capabilities2_fabric/package.xml +++ b/capabilities2_fabric/package.xml @@ -13,6 +13,7 @@ rclcpp capabilities2_msgs rclcpp_action + backward_ros tinyxml2 diff --git a/capabilities2_fabric/src/capabilities_fabric.cpp b/capabilities2_fabric/src/capabilities_fabric.cpp index 006bcc2..1a1f902 100644 --- a/capabilities2_fabric/src/capabilities_fabric.cpp +++ b/capabilities2_fabric/src/capabilities_fabric.cpp @@ -1,6 +1,6 @@ #include -int main(int argc, char* argv[]) +int main(int argc, char** argv) { rclcpp::init(argc, argv); @@ -9,6 +9,7 @@ int main(int argc, char* argv[]) // Initialize the node components after construction node->initialize(); + rclcpp::spin(node); rclcpp::shutdown(); diff --git a/capabilities2_server/CMakeLists.txt b/capabilities2_server/CMakeLists.txt index cb79c7f..3eb130c 100644 --- a/capabilities2_server/CMakeLists.txt +++ b/capabilities2_server/CMakeLists.txt @@ -19,6 +19,7 @@ find_package(pluginlib REQUIRED) find_package(rclcpp_components REQUIRED) find_package(capabilities2_msgs REQUIRED) find_package(capabilities2_runner REQUIRED) +find_package(backward_ros REQUIRED) find_package(PkgConfig) pkg_check_modules(UUID REQUIRED uuid) @@ -43,8 +44,10 @@ include_directories(include ${UUID_INCLUDE_DIRS} ) +# Component based implementation that compiles as a library + add_library(${PROJECT_NAME} SHARED - src/capabilities_server.cpp + src/capabilities_server_comp.cpp ) target_link_libraries(${PROJECT_NAME} @@ -69,19 +72,50 @@ ament_target_dependencies(${PROJECT_NAME} rclcpp_components_register_node(${PROJECT_NAME} PLUGIN "capabilities2_server::CapabilitiesServer" - EXECUTABLE capabilities2_server_node + EXECUTABLE capabilities2_server_component_node ) -ament_export_targets(capabilities2_server_node) +ament_export_targets(capabilities2_server_component_node) install(TARGETS ${PROJECT_NAME} - EXPORT capabilities2_server_node + EXPORT capabilities2_server_component_node ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin ) +# Non Component based implementation that compiles as a executable + +add_executable(${PROJECT_NAME}_node + src/capabilities_server_node.cpp +) + +target_link_libraries(${PROJECT_NAME}_node + ${UUID_LIBRARIES} + ${TINYXML2_LIB} + ${SQLITE3_LIBRARIES} + ${YAML_CPP_LIBRARIES} +) + +ament_target_dependencies(${PROJECT_NAME}_node + rclcpp + rclcpp_action + bondcpp + pluginlib + rclcpp_components + capabilities2_msgs + capabilities2_runner + SQLite3 + yaml-cpp + UUID +) + +install(TARGETS ${PROJECT_NAME}_node + DESTINATION lib/${PROJECT_NAME} +) + # make test executable + add_executable(test_capabilities_server test/test.cpp) target_link_libraries(test_capabilities_server ${PROJECT_NAME} diff --git a/capabilities2_server/include/capabilities2_server/capabilities_server.hpp b/capabilities2_server/include/capabilities2_server/capabilities_server.hpp index b5c5edd..517c065 100644 --- a/capabilities2_server/include/capabilities2_server/capabilities_server.hpp +++ b/capabilities2_server/include/capabilities2_server/capabilities_server.hpp @@ -60,7 +60,7 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI { public: CapabilitiesServer(const rclcpp::NodeOptions& options = rclcpp::NodeOptions()) - : Node("capabilities", options), CapabilitiesAPI() + : Node("capabilities2", options), CapabilitiesAPI() { // params interface // loop rate diff --git a/capabilities2_server/launch/server.launch.py b/capabilities2_server/launch/server.launch.py index a7b4fa8..6c63d94 100644 --- a/capabilities2_server/launch/server.launch.py +++ b/capabilities2_server/launch/server.launch.py @@ -5,8 +5,6 @@ import os from launch import LaunchDescription from launch_ros.actions import Node -from launch_ros.actions import ComposableNodeContainer -from launch_ros.descriptions import ComposableNode from ament_index_python.packages import get_package_share_directory @@ -20,21 +18,11 @@ def generate_launch_description(): server_config = os.path.join(get_package_share_directory('capabilities2_server'), 'config', 'capabilities.yaml') # create bridge composition - capabilities = ComposableNodeContainer( - name='capabilities2_container', - namespace='', - package='rclcpp_components', - executable='component_container_mt', - # prefix=['xterm -e gdb -ex run --args'], # Add GDB debugging prefix - arguments=['--ros-args', '--log-level', 'info'], - composable_node_descriptions=[ - ComposableNode( - package='capabilities2_server', - plugin='capabilities2_server::CapabilitiesServer', - name='capabilities', - parameters=[server_config] - ) - ] + capabilities2 = Node( + package='capabilities2_server', + executable='capabilities2_server_node', + name='capabilities', + parameters=[server_config] ) # create launch proxy node @@ -46,6 +34,6 @@ def generate_launch_description(): # return return LaunchDescription([ - capabilities, + capabilities2, launch_proxy ]) diff --git a/capabilities2_server/launch/server_composed.launch.py b/capabilities2_server/launch/server_composed.launch.py new file mode 100644 index 0000000..a7b4fa8 --- /dev/null +++ b/capabilities2_server/launch/server_composed.launch.py @@ -0,0 +1,51 @@ +''' +capabilities2_server launch file +''' + +import os +from launch import LaunchDescription +from launch_ros.actions import Node +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + """Generate launch description for capabilities2 server + + Returns: + LaunchDescription: The launch description for capabilities2 server + """ + # load config file + server_config = os.path.join(get_package_share_directory('capabilities2_server'), 'config', 'capabilities.yaml') + + # create bridge composition + capabilities = ComposableNodeContainer( + name='capabilities2_container', + namespace='', + package='rclcpp_components', + executable='component_container_mt', + # prefix=['xterm -e gdb -ex run --args'], # Add GDB debugging prefix + arguments=['--ros-args', '--log-level', 'info'], + composable_node_descriptions=[ + ComposableNode( + package='capabilities2_server', + plugin='capabilities2_server::CapabilitiesServer', + name='capabilities', + parameters=[server_config] + ) + ] + ) + + # create launch proxy node + launch_proxy = Node( + package='capabilities2_launch_py', + executable='capabilities2_launch_py', + name='capabilities2_launch_py' + ) + + # return + return LaunchDescription([ + capabilities, + launch_proxy + ]) diff --git a/capabilities2_server/package.xml b/capabilities2_server/package.xml index ffa1e48..e9fb76d 100644 --- a/capabilities2_server/package.xml +++ b/capabilities2_server/package.xml @@ -28,6 +28,7 @@ tinyxml2 libsqlite3-dev uuid + backward_ros rclpy launch_ros diff --git a/capabilities2_server/src/capabilities_server.cpp b/capabilities2_server/src/capabilities_server_comp.cpp similarity index 94% rename from capabilities2_server/src/capabilities_server.cpp rename to capabilities2_server/src/capabilities_server_comp.cpp index 8c8eb6f..efbad0e 100644 --- a/capabilities2_server/src/capabilities_server.cpp +++ b/capabilities2_server/src/capabilities_server_comp.cpp @@ -1,4 +1,4 @@ #include #include -RCLCPP_COMPONENTS_REGISTER_NODE(capabilities2_server::CapabilitiesServer) +RCLCPP_COMPONENTS_REGISTER_NODE(capabilities2_server::CapabilitiesServer) \ No newline at end of file diff --git a/capabilities2_server/src/capabilities_server_node.cpp b/capabilities2_server/src/capabilities_server_node.cpp new file mode 100644 index 0000000..48e5bb6 --- /dev/null +++ b/capabilities2_server/src/capabilities_server_node.cpp @@ -0,0 +1,22 @@ +#include + +int main(int argc, char** argv) +{ + // Initialize ROS 2 + rclcpp::init(argc, argv); + + // Create the node object and executor object + auto node = std::make_shared(); + auto exec = std::make_shared(); + + // Add the node to the executor + exec->add_node(node); + + // Spin the executor + exec->spin(); + + // Shutdown ROS 2 + rclcpp::shutdown(); + + return 0; +} \ No newline at end of file From 07acc1f46809059ef3ddde1f51469ae30aca5644 Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Mon, 13 Jan 2025 01:42:10 +1100 Subject: [PATCH 070/133] fixed statetransition error --- capabilities2_fabric/CMakeLists.txt | 2 + .../docs/waypoint_runner_ex1.md | 21 +----- .../docs/waypoint_runner_ex2.md | 21 +----- .../capabilities_fabric.hpp | 26 +++++--- .../capabilities_file_parser.hpp | 2 +- .../capabilities_xml_parser.hpp | 2 +- .../structs/bond_client.hpp | 65 +++++++++++++++++++ .../structs/connection.hpp | 0 .../src/capabilities_fabric.cpp | 2 +- .../src/capabilities_file_parser.cpp | 2 +- .../capabilities2_runner/action_runner.hpp | 3 - .../capabilities2_server/bond_cache.hpp | 4 +- docs/nav2_setup.md | 19 +++++- 13 files changed, 111 insertions(+), 58 deletions(-) rename capabilities2_fabric/include/{capabilities2_executor => capabilities2_fabric}/capabilities_fabric.hpp (98%) rename capabilities2_fabric/include/{capabilities2_executor => capabilities2_fabric}/capabilities_file_parser.hpp (98%) rename capabilities2_fabric/include/{capabilities2_executor => capabilities2_fabric}/capabilities_xml_parser.hpp (99%) create mode 100644 capabilities2_fabric/include/capabilities2_fabric/structs/bond_client.hpp rename capabilities2_fabric/include/{capabilities2_executor => capabilities2_fabric}/structs/connection.hpp (100%) diff --git a/capabilities2_fabric/CMakeLists.txt b/capabilities2_fabric/CMakeLists.txt index 36639d6..9faebc0 100644 --- a/capabilities2_fabric/CMakeLists.txt +++ b/capabilities2_fabric/CMakeLists.txt @@ -15,6 +15,7 @@ find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_action REQUIRED) find_package(capabilities2_msgs REQUIRED) +find_package(bondcpp REQUIRED) find_package(backward_ros REQUIRED) # Locate the static version of tinyxml2 @@ -39,6 +40,7 @@ target_link_libraries(${PROJECT_NAME} ament_target_dependencies(${PROJECT_NAME} rclcpp rclcpp_action + bondcpp capabilities2_msgs ) diff --git a/capabilities2_fabric/docs/waypoint_runner_ex1.md b/capabilities2_fabric/docs/waypoint_runner_ex1.md index 555c773..4e978e8 100644 --- a/capabilities2_fabric/docs/waypoint_runner_ex1.md +++ b/capabilities2_fabric/docs/waypoint_runner_ex1.md @@ -2,26 +2,7 @@ ### Dependencies -This example uses nav2 stack and turtlebot3. - -Run the following commands to install nav2 stack - -```bash -sudo apt install ros-humble-navigation2 ros-humble-nav2-bringup -``` - -Run the following commands to install turtlebot3 - -```bash -sudo apt install ros-humble-turtlebot3* -``` - -Clone the nav_stack default configuration and launch files to the same workspace if its not already availabe in the workspace. Capabilities2 Nav2 Runners are dependent on this package. - -```bash -cd src -git clone https://github.com/CollaborativeRoboticsLab/nav_stack.git -``` +This example uses nav2 stack and turtlebot3. Follow instructions from [Nav2 Dependency Installation](../../docs/nav2_setup.md) to setup nav stack. ### Plan selection diff --git a/capabilities2_fabric/docs/waypoint_runner_ex2.md b/capabilities2_fabric/docs/waypoint_runner_ex2.md index 01121e9..8a49adb 100644 --- a/capabilities2_fabric/docs/waypoint_runner_ex2.md +++ b/capabilities2_fabric/docs/waypoint_runner_ex2.md @@ -2,26 +2,7 @@ ### Dependencies -This example uses nav2 stack and turtlebot3. - -Run the following commands to install nav2 stack - -```bash -sudo apt install ros-humble-navigation2 ros-humble-nav2-bringup -``` - -Run the following commands to install turtlebot3 - -```bash -sudo apt install ros-humble-turtlebot3* -``` - -Clone the nav_stack default configuration and launch files to the same workspace if its not already availabe in the workspace. Capabilities2 Nav2 Runners are dependent on this package. - -```bash -cd src -git clone https://github.com/CollaborativeRoboticsLab/nav_stack.git -``` +This example uses nav2 stack and turtlebot3. Follow instructions from [Nav2 Dependency Installation](../../docs/nav2_setup.md) to setup nav stack. ### Plan selection diff --git a/capabilities2_fabric/include/capabilities2_executor/capabilities_fabric.hpp b/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric.hpp similarity index 98% rename from capabilities2_fabric/include/capabilities2_executor/capabilities_fabric.hpp rename to capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric.hpp index e6ff0de..6826ba4 100644 --- a/capabilities2_fabric/include/capabilities2_executor/capabilities_fabric.hpp +++ b/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric.hpp @@ -8,7 +8,8 @@ #include #include -#include +#include +#include #include @@ -591,12 +592,15 @@ class CapabilitiesFabric : public rclcpp::Node } auto response = future.get(); - bond_id = response->bond_id; + bond_id_ = response->bond_id; - feedback->progress = "Received the bond id : " + bond_id; + feedback->progress = "Received the bond id : " + bond_id_; goal_handle->publish_feedback(feedback); RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); + bond_client_ = std::make_unique(shared_from_this(), bond_id_); + bond_client_->start(); + expected_capabilities_ = connection_map.size(); RCLCPP_INFO(this->get_logger(), "Requsting start of %d capabilities", expected_capabilities_); @@ -608,9 +612,8 @@ class CapabilitiesFabric : public rclcpp::Node /** * @brief Request use of capability from capabilities2 server * - * @param capability capability name to be started + * @param capabilities capability list to be started * @param provider provider of the capability - * @param bond_id bond_id for the capability */ void use_capability(std::map& capabilities, const std::shared_ptr goal_handle) { @@ -622,7 +625,7 @@ class CapabilitiesFabric : public rclcpp::Node auto request_use = std::make_shared(); request_use->capability = capability; request_use->preferred_provider = provider; - request_use->bond_id = bond_id; + request_use->bond_id = bond_id_; feedback->progress = "Starting capability of Node " + std::to_string(completed_capabilities_) + " named " + capabilities[completed_capabilities_].source.runner; @@ -701,7 +704,7 @@ class CapabilitiesFabric : public rclcpp::Node auto request_free = std::make_shared(); request_free->capability = capability; - request_free->bond_id = bond_id; + request_free->bond_id = bond_id_; // send the request auto result_future = free_capability_client_->async_send_request( @@ -721,6 +724,8 @@ class CapabilitiesFabric : public rclcpp::Node feedback->progress = "Successfully freed capability " + capability; goal_handle->publish_feedback(feedback); RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); + + bond_client_->stop(); }); } @@ -921,8 +926,11 @@ class CapabilitiesFabric : public rclcpp::Node int expected_configurations_; int completed_configurations_; - /** Bond ID between capabilities server and this client */ - std::string bond_id; + /** Bond id */ + std::string bond_id_; + + /** Bond between capabilities server and this client */ + std::shared_ptr bond_client_; /** XML Document */ tinyxml2::XMLDocument document; diff --git a/capabilities2_fabric/include/capabilities2_executor/capabilities_file_parser.hpp b/capabilities2_fabric/include/capabilities2_fabric/capabilities_file_parser.hpp similarity index 98% rename from capabilities2_fabric/include/capabilities2_executor/capabilities_file_parser.hpp rename to capabilities2_fabric/include/capabilities2_fabric/capabilities_file_parser.hpp index 8ca636a..d7614e9 100644 --- a/capabilities2_fabric/include/capabilities2_executor/capabilities_file_parser.hpp +++ b/capabilities2_fabric/include/capabilities2_fabric/capabilities_file_parser.hpp @@ -10,7 +10,7 @@ #include #include -#include +#include #include /** diff --git a/capabilities2_fabric/include/capabilities2_executor/capabilities_xml_parser.hpp b/capabilities2_fabric/include/capabilities2_fabric/capabilities_xml_parser.hpp similarity index 99% rename from capabilities2_fabric/include/capabilities2_executor/capabilities_xml_parser.hpp rename to capabilities2_fabric/include/capabilities2_fabric/capabilities_xml_parser.hpp index fedbc53..38ff3d3 100644 --- a/capabilities2_fabric/include/capabilities2_executor/capabilities_xml_parser.hpp +++ b/capabilities2_fabric/include/capabilities2_fabric/capabilities_xml_parser.hpp @@ -2,7 +2,7 @@ #include #include #include -#include +#include namespace capabilities2_xml_parser { diff --git a/capabilities2_fabric/include/capabilities2_fabric/structs/bond_client.hpp b/capabilities2_fabric/include/capabilities2_fabric/structs/bond_client.hpp new file mode 100644 index 0000000..b1f286a --- /dev/null +++ b/capabilities2_fabric/include/capabilities2_fabric/structs/bond_client.hpp @@ -0,0 +1,65 @@ +#include +#include + +class BondClient +{ +public: + BondClient(rclcpp::Node::SharedPtr node, const std::string& bond_id, const std::string& bonds_topic = "/capabilities/bond") + { + bonds_topic_ = bonds_topic; + bond_id_ = bond_id; + node_ = node; + } + + void start() + { + RCLCPP_INFO(node_->get_logger(), "[BondClient] creating bond to capabilities server"); + + bond_ = + std::make_unique(bonds_topic_, bond_id_, node_, std::bind(&BondClient::on_broken, this), std::bind(&BondClient::on_formed, this)); + + bond_->setHeartbeatPeriod(0.10); + bond_->setHeartbeatTimeout(10.0); + bond_->start(); + } + + void stop() + { + RCLCPP_INFO(node_->get_logger(), "[BondClient] destroying bond to capabilities server"); + + if (bond_) + { + bond_.reset(); + } + } + + ~BondClient() + { + stop(); + } + +private: + void on_formed() + { + // log bond established event + RCLCPP_INFO(node_->get_logger(), "[BondClient] bond with capabilities server formed with id: %s", bond_id_.c_str()); + } + + void on_broken() + { + // log bond established event + RCLCPP_INFO(node_->get_logger(), "[BondClient] bond with capabilities server broken with id: %s", bond_id_.c_str()); + } + + /** Ros node pointer */ + rclcpp::Node::SharedPtr node_; + + /** Bond id string */ + std::string bond_id_; + + /** Bond topic to be published */ + std::string bonds_topic_; + + /** Heart beat bond with capabilities server */ + std::shared_ptr bond_; +}; \ No newline at end of file diff --git a/capabilities2_fabric/include/capabilities2_executor/structs/connection.hpp b/capabilities2_fabric/include/capabilities2_fabric/structs/connection.hpp similarity index 100% rename from capabilities2_fabric/include/capabilities2_executor/structs/connection.hpp rename to capabilities2_fabric/include/capabilities2_fabric/structs/connection.hpp diff --git a/capabilities2_fabric/src/capabilities_fabric.cpp b/capabilities2_fabric/src/capabilities_fabric.cpp index 1a1f902..e89ccbb 100644 --- a/capabilities2_fabric/src/capabilities_fabric.cpp +++ b/capabilities2_fabric/src/capabilities_fabric.cpp @@ -1,4 +1,4 @@ -#include +#include int main(int argc, char** argv) { diff --git a/capabilities2_fabric/src/capabilities_file_parser.cpp b/capabilities2_fabric/src/capabilities_file_parser.cpp index aa0d9b0..bb08cf0 100644 --- a/capabilities2_fabric/src/capabilities_file_parser.cpp +++ b/capabilities2_fabric/src/capabilities_file_parser.cpp @@ -1,4 +1,4 @@ -#include +#include int main(int argc, char* argv[]) { diff --git a/capabilities2_runner/include/capabilities2_runner/action_runner.hpp b/capabilities2_runner/include/capabilities2_runner/action_runner.hpp index 1b2e82b..f308821 100644 --- a/capabilities2_runner/include/capabilities2_runner/action_runner.hpp +++ b/capabilities2_runner/include/capabilities2_runner/action_runner.hpp @@ -94,9 +94,6 @@ class ActionRunner : public RunnerBase // Check if the cancel operation is complete if (cancel_future.wait_for(std::chrono::milliseconds(100)) == std::future_status::ready) break; - - // Spin some work on the node to keep the callback within scope - rclcpp::spin_some(node_); } } catch (const rclcpp_action::exceptions::UnknownGoalHandleError& e) diff --git a/capabilities2_server/include/capabilities2_server/bond_cache.hpp b/capabilities2_server/include/capabilities2_server/bond_cache.hpp index 296ddcc..4895107 100644 --- a/capabilities2_server/include/capabilities2_server/bond_cache.hpp +++ b/capabilities2_server/include/capabilities2_server/bond_cache.hpp @@ -19,7 +19,7 @@ namespace capabilities2_server class BondCache { public: - BondCache(const std::string& bonds_topic = "~/bonds") : bonds_topic_(bonds_topic) + BondCache(const std::string& bonds_topic = "/capabilities/bond") : bonds_topic_(bonds_topic) { } @@ -124,6 +124,8 @@ class BondCache live_bonds_[bond_id] = std::make_unique(bonds_topic_, bond_id, node, on_broken, on_formed); // start the bond + live_bonds_[bond_id]->setHeartbeatPeriod(0.10); + live_bonds_[bond_id]->setHeartbeatTimeout(10.0); live_bonds_[bond_id]->start(); } diff --git a/docs/nav2_setup.md b/docs/nav2_setup.md index 1b27fe5..2a44385 100644 --- a/docs/nav2_setup.md +++ b/docs/nav2_setup.md @@ -2,8 +2,16 @@ ## Install nav2 stack +Run the following commands to install nav2 stack + +```bash +sudo apt install ros-humble-navigation2 ros-humble-nav2-bringup +``` + +## Install slam-toolbox + ```bash -sudo apt install ros-$ROS_DISTRO-navigation2 ros-$ROS_DISTRO-nav2-bringup ros-$ROS_DISTRO-slam-toolbox +sudo apt install ros-$ROS_DISTRO-slam-toolbox ``` ## Clone configuration @@ -16,8 +24,17 @@ git clone https://github.com/CollaborativeRoboticsLab/nav_stack.git ``` ## Turtlebot3 Simulation (Optional) + If using a simulated turtlebot3 for testing, install using following commands. ```bash sudo apt install ros-$ROS_DISTRO-turtlebot3* +``` + +## Dependency Installation + +Move to workspace root and run the following command to install dependencies + +```bash +rosdep install --from-paths src --ignore-src -r -y ``` \ No newline at end of file From 28fac047df4c9cd297b2f44f3a4c1c3dcd52149a Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Mon, 13 Jan 2025 21:20:47 +1100 Subject: [PATCH 071/133] refactored the fabric code --- .../capabilities_fabric.hpp | 544 +++++++----------- .../capabilities_xml_parser.hpp | 2 +- .../{structs => utils}/bond_client.hpp | 0 .../{structs => utils}/connection.hpp | 0 4 files changed, 203 insertions(+), 343 deletions(-) rename capabilities2_fabric/include/capabilities2_fabric/{structs => utils}/bond_client.hpp (100%) rename capabilities2_fabric/include/capabilities2_fabric/{structs => utils}/connection.hpp (100%) diff --git a/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric.hpp b/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric.hpp index 6826ba4..3deec48 100644 --- a/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric.hpp +++ b/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric.hpp @@ -9,7 +9,7 @@ #include #include -#include +#include #include @@ -76,73 +76,21 @@ class CapabilitiesFabric : public rclcpp::Node establish_bond_client_ = this->create_client("/capabilities/establish_bond"); use_capability_client_ = this->create_client("/capabilities/use_capability"); free_capability_client_ = this->create_client("/capabilities/free_capability"); - trigger_capability_client_ = this->create_client("/capabilities/trigger_capability"); - configure_capability_client_ = this->create_client("/capabilities/configure_capability"); + trig_capability_client_ = this->create_client("/capabilities/trigger_capability"); + conf_capability_client_ = this->create_client("/capabilities/configure_capability"); // Wait for services to become available - while (!get_interfaces_client_->wait_for_service(std::chrono::seconds(1))) - { - RCLCPP_ERROR(this->get_logger(), "/capabilities/get_interfaces not available"); - rclcpp::shutdown(); - return; - } - RCLCPP_INFO(this->get_logger(), "/capabilities/get_interfaces connected"); - - while (!get_sem_interf_client_->wait_for_service(std::chrono::seconds(1))) - { - RCLCPP_ERROR(this->get_logger(), "/capabilities/get_semantic_interfaces not available"); - rclcpp::shutdown(); - return; - } - RCLCPP_INFO(this->get_logger(), "/capabilities/get_semantic_interfaces connected"); - - while (!get_providers_client_->wait_for_service(std::chrono::seconds(1))) - { - RCLCPP_ERROR(this->get_logger(), "/capabilities/get_providers not available"); - rclcpp::shutdown(); - return; - } - RCLCPP_INFO(this->get_logger(), "/capabilities/get_providers connected"); - - while (!establish_bond_client_->wait_for_service(std::chrono::seconds(1))) - { - RCLCPP_ERROR(this->get_logger(), "/capabilities/establish_bond not available"); - rclcpp::shutdown(); - return; - } - RCLCPP_INFO(this->get_logger(), "/capabilities/establish_bond connected"); - - while (!use_capability_client_->wait_for_service(std::chrono::seconds(1))) - { - RCLCPP_ERROR(this->get_logger(), "/capabilities/use_capability not available"); - rclcpp::shutdown(); - return; - } - RCLCPP_INFO(this->get_logger(), "/capabilities/use_capability connected"); - - while (!free_capability_client_->wait_for_service(std::chrono::seconds(1))) - { - RCLCPP_ERROR(this->get_logger(), "/capabilities/free_capability not available"); - rclcpp::shutdown(); - return; - } - RCLCPP_INFO(this->get_logger(), "/capabilities/free_capability connected"); - - while (!trigger_capability_client_->wait_for_service(std::chrono::seconds(1))) - { - RCLCPP_ERROR(this->get_logger(), "/capabilities/trigger_capability not available"); - rclcpp::shutdown(); - return; - } - RCLCPP_INFO(this->get_logger(), "/capabilities/trigger_capability connected"); - - while (!configure_capability_client_->wait_for_service(std::chrono::seconds(1))) - { - RCLCPP_ERROR(this->get_logger(), "/capabilities/configure_capability not available"); - rclcpp::shutdown(); - return; - } - RCLCPP_INFO(this->get_logger(), "/capabilities/configure_capability connected"); + check_service(!get_interfaces_client_->wait_for_service(std::chrono::seconds(1)), "/capabilities/get_interfaces"); + check_service(!get_sem_interf_client_->wait_for_service(std::chrono::seconds(1)), "/capabilities/get_semantic_interfaces"); + check_service(!get_providers_client_->wait_for_service(std::chrono::seconds(1)), "/capabilities/get_providers"); + check_service(!establish_bond_client_->wait_for_service(std::chrono::seconds(1)), "/capabilities/establish_bond"); + check_service(!use_capability_client_->wait_for_service(std::chrono::seconds(1)), "/capabilities/use_capability"); + check_service(!free_capability_client_->wait_for_service(std::chrono::seconds(1)), "/capabilities/free_capability"); + check_service(!trig_capability_client_->wait_for_service(std::chrono::seconds(1)), "/capabilities/trigger_capability"); + check_service(!conf_capability_client_->wait_for_service(std::chrono::seconds(1)), "/capabilities/configure_capability"); + + feedback_msg = std::make_shared(); + result_msg = std::make_shared(); } private: @@ -203,11 +151,7 @@ class CapabilitiesFabric : public rclcpp::Node */ void execution(const std::shared_ptr goal_handle) { - auto feedback = std::make_shared(); - - feedback->progress = "Execution started"; - goal_handle->publish_feedback(feedback); - RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); + print_and_feedback(goal_handle, "Execution started", false); expected_providers_ = 0; completed_providers_ = 0; @@ -229,43 +173,29 @@ class CapabilitiesFabric : public rclcpp::Node */ void getInterfaces(const std::shared_ptr goal_handle) { - auto feedback = std::make_shared(); - - feedback->progress = "Requesting Interface information"; - goal_handle->publish_feedback(feedback); - RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); + print_and_feedback(goal_handle, "Requesting Interface information", false); auto request_interface = std::make_shared(); // request data from the server - auto result_future = - get_interfaces_client_->async_send_request(request_interface, [this, goal_handle, feedback](GetInterfacesClient::SharedFuture future) { - auto result = std::make_shared(); - - if (!future.valid()) - { - result->success = false; - result->message = "Failed to get Interface information"; - goal_handle->abort(result); - RCLCPP_ERROR(this->get_logger(), result->message.c_str()); - - RCLCPP_ERROR(this->get_logger(), "Server Execution Cancelled"); - return; - } - - auto response = future.get(); + auto result_future = get_interfaces_client_->async_send_request(request_interface, [this, goal_handle](GetInterfacesClient::SharedFuture future) { + auto result = std::make_shared(); - feedback->progress = "Received Interface information"; - goal_handle->publish_feedback(feedback); - RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); + if (!future.valid()) + { + print_and_result(goal_handle, "Failed to get Interface information. Server execution cancelled", false); + return; + } - expected_interfaces_ = response->interfaces.size(); + auto response = future.get(); + expected_interfaces_ = response->interfaces.size(); - RCLCPP_INFO(this->get_logger(), "Requsting Semantic Interface information for %d interfaces", expected_interfaces_); + status = "Received Interfaces. Requsting " + std::to_string(expected_interfaces_) + " semantic interface information"; + print_and_feedback(goal_handle, status, false); - // Request each interface recursively for Semantic interfaces - getSemanticInterfaces(response->interfaces, goal_handle); - }); + // Request each interface recursively for Semantic interfaces + getSemanticInterfaces(response->interfaces, goal_handle); + }); } /** @@ -273,29 +203,20 @@ class CapabilitiesFabric : public rclcpp::Node */ void getSemanticInterfaces(const std::vector& interfaces, const std::shared_ptr goal_handle) { - auto feedback = std::make_shared(); - std::string requested_interface = interfaces[completed_interfaces_]; - RCLCPP_INFO(this->get_logger(), ""); - feedback->progress = "Requesting semantic interfaces for " + requested_interface; - goal_handle->publish_feedback(feedback); - RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); + status = "Requesting semantic interfaces for " + requested_interface; + print_and_feedback(goal_handle, status, true); auto request_semantic = std::make_shared(); request_semantic->interface = requested_interface; // request semantic interface from the server auto result_semantic_future = get_sem_interf_client_->async_send_request( - request_semantic, [this, goal_handle, feedback, interfaces, requested_interface](GetSemanticInterfacesClient::SharedFuture future) { + request_semantic, [this, goal_handle, interfaces, requested_interface](GetSemanticInterfacesClient::SharedFuture future) { if (!future.valid()) { - auto result = std::make_shared(); - result->success = false; - result->message = "Failed to get Semantic Interface information"; - goal_handle->abort(result); - - RCLCPP_ERROR(this->get_logger(), "Failed to get Semantic Interface information. Server Execution Cancelled"); + print_and_result(goal_handle, "Failed to get Semantic Interface information. Server execution cancelled", false); return; } @@ -310,10 +231,9 @@ class CapabilitiesFabric : public rclcpp::Node interface_list.push_back(semantic_interface); is_semantic_list.push_back(true); - feedback->progress = std::to_string(completed_interfaces_) + "/" + std::to_string(expected_interfaces_) + " : Received " + - semantic_interface + " for " + requested_interface + ". So added " + semantic_interface; - goal_handle->publish_feedback(feedback); - RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); + status = (completed_interfaces_) + "/" + std::to_string(expected_interfaces_) + " : Received " + semantic_interface + " for " + + requested_interface + ". So added " + semantic_interface; + print_and_feedback(goal_handle, status, false); } } // if no semantic interfaces are availble for a given interface, add the interface instead @@ -322,10 +242,9 @@ class CapabilitiesFabric : public rclcpp::Node interface_list.push_back(requested_interface); is_semantic_list.push_back(false); - feedback->progress = std::to_string(completed_interfaces_) + "/" + std::to_string(expected_interfaces_) + " : Received none for " + - requested_interface + ". So added " + requested_interface; - goal_handle->publish_feedback(feedback); - RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); + status = std::to_string(completed_interfaces_) + "/" + std::to_string(expected_interfaces_) + " : Received none for " + + requested_interface + ". So added " + requested_interface; + print_and_feedback(goal_handle, status, false); } if (completed_interfaces_ != expected_interfaces_) @@ -335,14 +254,12 @@ class CapabilitiesFabric : public rclcpp::Node } else { - feedback->progress = "Received all requested Interface information"; - goal_handle->publish_feedback(feedback); - RCLCPP_INFO(this->get_logger(), ""); - RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); + print_and_feedback(goal_handle, "Received all requested Interface information", true); expected_providers_ = interface_list.size(); - RCLCPP_INFO(this->get_logger(), "Requsting Provider information for %d providers", expected_providers_); + status = "Requsting Provider information for " + std::to_string(expected_providers_) + " providers"; + print_and_feedback(goal_handle, status, false); // request providers from the interfaces in the interfaces_list getProvider(interface_list, is_semantic_list, goal_handle); @@ -360,12 +277,8 @@ class CapabilitiesFabric : public rclcpp::Node std::string requested_interface = interfaces[completed_providers_]; bool semantic_flag = is_semantic[completed_providers_]; - auto feedback = std::make_shared(); - - RCLCPP_INFO(this->get_logger(), ""); - feedback->progress = "Requesting provider for " + requested_interface; - goal_handle->publish_feedback(feedback); - RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); + status = "Requesting provider for " + requested_interface; + print_and_feedback(goal_handle, status, true); auto request_providers = std::make_shared(); @@ -374,15 +287,11 @@ class CapabilitiesFabric : public rclcpp::Node request_providers->include_semantic = semantic_flag; auto result_providers_future = get_providers_client_->async_send_request( - request_providers, [this, is_semantic, requested_interface, interfaces, goal_handle, feedback](GetProvidersClient::SharedFuture future) { + request_providers, [this, is_semantic, requested_interface, interfaces, goal_handle](GetProvidersClient::SharedFuture future) { if (!future.valid()) { - auto result = std::make_shared(); - result->success = false; - result->message = "Did not retrieve providers for interface: " + requested_interface; - goal_handle->abort(result); - - RCLCPP_ERROR(this->get_logger(), result->message.c_str()); + status = "Did not retrieve providers for interface: " + requested_interface; + print_and_result(goal_handle, status, false); return; } @@ -394,10 +303,9 @@ class CapabilitiesFabric : public rclcpp::Node // add defualt provider to the list providers_list.push_back(response->default_provider); - feedback->progress = std::to_string(completed_providers_) + "/" + std::to_string(expected_providers_) + " : Received " + - response->default_provider + " for " + requested_interface + ". So added " + response->default_provider; - goal_handle->publish_feedback(feedback); - RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); + status = std::to_string(completed_providers_) + "/" + std::to_string(expected_providers_) + " : Received " + response->default_provider + + " for " + requested_interface + ". So added " + response->default_provider; + print_and_feedback(goal_handle, status, false); } // add additional providers to the list if available @@ -406,18 +314,16 @@ class CapabilitiesFabric : public rclcpp::Node for (const auto& provider : response->providers) { providers_list.push_back(provider); - feedback->progress = std::to_string(completed_providers_) + "/" + std::to_string(expected_providers_) + " : Received and added " + - provider + " for " + requested_interface; - goal_handle->publish_feedback(feedback); - RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); + + status = std::to_string(completed_providers_) + "/" + std::to_string(expected_providers_) + " : Received and added " + provider + + " for " + requested_interface; + print_and_feedback(goal_handle, status, false); } } else { - feedback->progress = - std::to_string(completed_providers_) + "/" + std::to_string(expected_providers_) + " : No providers for " + requested_interface; - goal_handle->publish_feedback(feedback); - RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); + status = std::to_string(completed_providers_) + "/" + std::to_string(expected_providers_) + " : No providers for " + requested_interface; + print_and_feedback(goal_handle, status, false); } // Check if all expected calls are completed before calling verify_plan @@ -428,12 +334,7 @@ class CapabilitiesFabric : public rclcpp::Node } else { - auto feedback = std::make_shared(); - - RCLCPP_INFO(this->get_logger(), ""); - feedback->progress = "All requested interface, semantic interface and provider data recieved"; - goal_handle->publish_feedback(feedback); - RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); + print_and_feedback(goal_handle, "All requested interface, semantic interface and provider data recieved", true); verify_and_continue(goal_handle); } @@ -446,23 +347,17 @@ class CapabilitiesFabric : public rclcpp::Node */ void verify_and_continue(const std::shared_ptr goal_handle) { - auto feedback = std::make_shared(); - feedback->progress = "Verifying the plan"; - goal_handle->publish_feedback(feedback); - RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); + print_and_feedback(goal_handle, "Verifying the plan", false); // verify the plan if (!verify_plan(goal_handle)) { - feedback->progress = "Plan verification failed"; - goal_handle->publish_feedback(feedback); - RCLCPP_ERROR(this->get_logger(), feedback->progress.c_str()); - - auto result = std::make_shared(); + print_and_feedback(goal_handle, "Plan verification failed", false); if (rejected_list.size() > 0) { // TODO: improve with error codes + auto result = std::make_shared(); result->success = false; result->message = "Plan verification failed. There are mismatched events"; @@ -478,32 +373,23 @@ class CapabilitiesFabric : public rclcpp::Node else { // TODO: improve with error codes - result->success = false; - result->message = "Plan verification failed."; - goal_handle->abort(result); - RCLCPP_ERROR(this->get_logger(), "Server Execution Cancelled"); + print_and_result(goal_handle, "Plan verification failed. Server Execution Cancelled.", false); } RCLCPP_ERROR(this->get_logger(), "Server Execution Cancelled"); } - feedback->progress = "Plan verification successful"; - goal_handle->publish_feedback(feedback); - RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); + print_and_feedback(goal_handle, "Plan verification successful", false); // extract the plan from the XMLDocument tinyxml2::XMLElement* plan = capabilities2_xml_parser::get_plan(document); - feedback->progress = "Plan conversion successful"; - goal_handle->publish_feedback(feedback); - RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); + print_and_feedback(goal_handle, "Plan conversion successful", false); // Extract the connections from the plan capabilities2_xml_parser::extract_connections(plan, connection_map); - feedback->progress = "Connection extraction successful"; - goal_handle->publish_feedback(feedback); - RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); + print_and_feedback(goal_handle, "Connection extraction successful", false); // estasblish the bond with the server establish_bond(goal_handle); @@ -526,37 +412,25 @@ class CapabilitiesFabric : public rclcpp::Node // verify whether document got 'plan' tags if (!capabilities2_xml_parser::check_plan_tag(document)) { - result->success = false; - result->message = "Execution plan is not compatible. Please recheck and update"; - goal_handle->abort(result); - RCLCPP_ERROR(this->get_logger(), result->message.c_str()); + print_and_result(goal_handle, "Execution plan is not compatible. Please recheck and update", false); return false; } - feedback->progress = "'Plan' tag checking successful"; - goal_handle->publish_feedback(feedback); - RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); + print_and_feedback(goal_handle, "'Plan' tag checking successful", false); // extract the components within the 'plan' tags tinyxml2::XMLElement* plan = capabilities2_xml_parser::get_plan(document); - feedback->progress = "Plan extraction complete"; - goal_handle->publish_feedback(feedback); - RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); + print_and_feedback(goal_handle, "Plan extraction complete", false); // verify whether the plan is valid if (!capabilities2_xml_parser::check_tags(this->get_logger(), plan, interface_list, providers_list, control_tag_list, rejected_list)) { - result->success = false; - result->message = "Execution plan is faulty. Please recheck and update"; - goal_handle->abort(result); - RCLCPP_ERROR(this->get_logger(), result->message.c_str()); + print_and_result(goal_handle, "Execution plan is faulty. Please recheck and update", false); return false; } - feedback->progress = "Checking tags successful"; - goal_handle->publish_feedback(feedback); - RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); + print_and_feedback(goal_handle, "Checking tags successful", false); return true; } @@ -566,47 +440,35 @@ class CapabilitiesFabric : public rclcpp::Node */ void establish_bond(const std::shared_ptr goal_handle) { - auto feedback = std::make_shared(); - feedback->progress = "Requesting bond id"; - goal_handle->publish_feedback(feedback); - - RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); + print_and_feedback(goal_handle, "Requesting bond id", false); // create bond establishing server request auto request_bond = std::make_shared(); // send the request - auto result_future = - establish_bond_client_->async_send_request(request_bond, [this, goal_handle, feedback](EstablishBondClient::SharedFuture future) { - if (!future.valid()) - { - RCLCPP_ERROR(this->get_logger(), "Failed to receive the bond id"); - - auto result = std::make_shared(); - result->success = false; - result->message = "Failed to retrieve the bond id"; - RCLCPP_ERROR(this->get_logger(), "Server Execution Cancelled"); - - goal_handle->abort(result); - return; - } + auto result_future = establish_bond_client_->async_send_request(request_bond, [this, goal_handle](EstablishBondClient::SharedFuture future) { + if (!future.valid()) + { + print_and_result(goal_handle, "Failed to retrieve the bond id. Server execution cancelled", false); + return; + } - auto response = future.get(); - bond_id_ = response->bond_id; + auto response = future.get(); + bond_id_ = response->bond_id; - feedback->progress = "Received the bond id : " + bond_id_; - goal_handle->publish_feedback(feedback); - RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); + status = "Received the bond id : " + bond_id_; + print_and_feedback(goal_handle, status, false); - bond_client_ = std::make_unique(shared_from_this(), bond_id_); - bond_client_->start(); + bond_client_ = std::make_unique(shared_from_this(), bond_id_); + bond_client_->start(); - expected_capabilities_ = connection_map.size(); + expected_capabilities_ = connection_map.size(); - RCLCPP_INFO(this->get_logger(), "Requsting start of %d capabilities", expected_capabilities_); + status = "Requsting start of " + std::to_string(expected_capabilities_) + "capabilities"; + print_and_feedback(goal_handle, status, false); - use_capability(connection_map, goal_handle); - }); + use_capability(connection_map, goal_handle); + }); } /** @@ -617,8 +479,6 @@ class CapabilitiesFabric : public rclcpp::Node */ void use_capability(std::map& capabilities, const std::shared_ptr goal_handle) { - auto feedback = std::make_shared(); - std::string capability = capabilities[completed_capabilities_].source.runner; std::string provider = capabilities[completed_capabilities_].source.provider; @@ -627,70 +487,53 @@ class CapabilitiesFabric : public rclcpp::Node request_use->preferred_provider = provider; request_use->bond_id = bond_id_; - feedback->progress = - "Starting capability of Node " + std::to_string(completed_capabilities_) + " named " + capabilities[completed_capabilities_].source.runner; - goal_handle->publish_feedback(feedback); - RCLCPP_INFO(this->get_logger(), ""); - RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); + status = "Starting capability of Node " + std::to_string(completed_capabilities_) + " : " + capabilities[completed_capabilities_].source.runner; + print_and_feedback(goal_handle, status, true); // send the request - auto result_future = use_capability_client_->async_send_request(request_use, [this, goal_handle, feedback, capability, - provider](UseCapabilityClient::SharedFuture future) { - if (!future.valid()) - { - auto result = std::make_shared(); - result->success = false; - result->message = "Failed to Use capability " + capability + " from " + provider; - goal_handle->abort(result); - - RCLCPP_ERROR(this->get_logger(), result->message.c_str()); - RCLCPP_ERROR(this->get_logger(), "Server Execution Cancelled"); - - // release all capabilities that were used since not all started successfully - for (const auto& [key, value] : connection_map) - { - feedback->progress = "Freeing capability of Node " + std::to_string(key) + " named " + value.source.runner; - goal_handle->publish_feedback(feedback); - RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); + auto result_future = + use_capability_client_->async_send_request(request_use, [this, goal_handle, capability, provider](UseCapabilityClient::SharedFuture future) { + if (!future.valid()) + { + status = "Failed to Use capability " + capability + " from " + provider + ". Server Execution Cancelled"; + print_and_result(goal_handle, status, false); - free_capability(value.source.runner, goal_handle); - } + // release all capabilities that were used since not all started successfully + for (const auto& [key, value] : connection_map) + { + status = "Freeing capability of Node " + std::to_string(key) + " named " + value.source.runner; + print_and_feedback(goal_handle, status, false); - return; - } + free_capability(value.source.runner, goal_handle); + } - completed_capabilities_++; + return; + } - auto response = future.get(); + completed_capabilities_++; - feedback->progress = std::to_string(completed_capabilities_) + "/" + std::to_string(expected_capabilities_) + " : Capability start succeessful"; - goal_handle->publish_feedback(feedback); - RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); + auto response = future.get(); - // Check if all expected calls are completed before calling verify_plan - if (completed_capabilities_ == expected_capabilities_) - { - RCLCPP_INFO(this->get_logger(), ""); - feedback->progress = "All requested capabilities have been used"; - goal_handle->publish_feedback(feedback); - RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); + status = std::to_string(completed_capabilities_) + "/" + std::to_string(expected_capabilities_) + " : start succeessful"; + print_and_feedback(goal_handle, status, true); - feedback->progress = "Configuring the capabilities with events"; - goal_handle->publish_feedback(feedback); - RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); + // Check if all expected calls are completed before calling verify_plan + if (completed_capabilities_ == expected_capabilities_) + { + print_and_feedback(goal_handle, "All requested capabilities have been started. Configuring the capabilities with events", true); - expected_configurations_ = connection_map.size(); + expected_configurations_ = connection_map.size(); - RCLCPP_INFO(this->get_logger(), ""); - RCLCPP_INFO(this->get_logger(), "Requsting capability configuration for %d capabilities", expected_configurations_); + status = "Requsting capability configuration for " + std::to_string(expected_configurations_) + "capabilities"; + print_and_feedback(goal_handle, status, true); - configure_capabilities(connection_map, goal_handle); - } - else - { - use_capability(connection_map, goal_handle); - } - }); + configure_capabilities(connection_map, goal_handle); + } + else + { + use_capability(connection_map, goal_handle); + } + }); } /** @@ -700,30 +543,24 @@ class CapabilitiesFabric : public rclcpp::Node */ void free_capability(const std::string& capability, const std::shared_ptr goal_handle) { - auto feedback = std::make_shared(); - auto request_free = std::make_shared(); request_free->capability = capability; request_free->bond_id = bond_id_; // send the request - auto result_future = free_capability_client_->async_send_request( - request_free, [this, goal_handle, capability, feedback](FreeCapabilityClient::SharedFuture future) { + auto result_future = + free_capability_client_->async_send_request(request_free, [this, goal_handle, capability](FreeCapabilityClient::SharedFuture future) { if (!future.valid()) { - auto result = std::make_shared(); - result->success = false; - result->message = "Failed to free capability " + capability; - goal_handle->abort(result); - - RCLCPP_ERROR(this->get_logger(), result->message.c_str()); + status = "Failed to free capability " + capability; + print_and_result(goal_handle, status, false); return; } auto response = future.get(); - feedback->progress = "Successfully freed capability " + capability; - goal_handle->publish_feedback(feedback); - RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); + + status = "Successfully freed capability " + capability; + print_and_feedback(goal_handle, status, true); bond_client_->stop(); }); @@ -734,14 +571,11 @@ class CapabilitiesFabric : public rclcpp::Node */ void configure_capabilities(std::map& capabilities, const std::shared_ptr goal_handle) { - auto feedback = std::make_shared(); auto request_configure = std::make_shared(); - RCLCPP_INFO(this->get_logger(), ""); - feedback->progress = "Configuring capability of Node " + std::to_string(completed_configurations_) + " named " + - capabilities[completed_configurations_].source.runner; - goal_handle->publish_feedback(feedback); - RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); + status = "Configuring capability of Node " + std::to_string(completed_configurations_) + " named " + + capabilities[completed_configurations_].source.runner; + print_and_feedback(goal_handle, status, true); if (capabilities2_xml_parser::convert_to_string(capabilities[completed_configurations_].source.parameters, request_configure->source.parameters)) { @@ -820,17 +654,12 @@ class CapabilitiesFabric : public rclcpp::Node std::string source_capability = capabilities[completed_configurations_].source.runner; // send the request - auto result_future = configure_capability_client_->async_send_request( - request_configure, [this, goal_handle, source_capability, feedback](ConfigureCapabilityClient::SharedFuture future) { + auto result_future = conf_capability_client_->async_send_request( + request_configure, [this, goal_handle, source_capability](ConfigureCapabilityClient::SharedFuture future) { if (!future.valid()) { - auto result = std::make_shared(); - result->success = false; - result->message = "Failed to configure capability :" + source_capability; - goal_handle->abort(result); - - RCLCPP_ERROR(this->get_logger(), result->message.c_str()); - RCLCPP_ERROR(this->get_logger(), "Server Execution Cancelled"); + status = "Failed to configure capability :" + source_capability + ". Server execution cancelled"; + print_and_result(goal_handle, status, false); return; } @@ -838,24 +667,14 @@ class CapabilitiesFabric : public rclcpp::Node auto response = future.get(); - feedback->progress = std::to_string(completed_configurations_) + "/" + std::to_string(expected_configurations_) + - " : Successfully configured capability : " + source_capability; - goal_handle->publish_feedback(feedback); - RCLCPP_INFO(this->get_logger(), ""); - RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); + status = std::to_string(completed_configurations_) + "/" + std::to_string(expected_configurations_) + + " : Successfully configured capability : " + source_capability; + print_and_feedback(goal_handle, status, true); // Check if all expected calls are completed before calling verify_plan if (completed_configurations_ == expected_configurations_) { - RCLCPP_INFO(this->get_logger(), ""); - feedback->progress = "All requested capabilities have been configured"; - goal_handle->publish_feedback(feedback); - RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); - - RCLCPP_INFO(this->get_logger(), ""); - feedback->progress = "Triggering the first capability"; - goal_handle->publish_feedback(feedback); - RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); + print_and_feedback(goal_handle, "All requested capabilities have been configured. Triggering the first capability", true); trigger_first_node(goal_handle); } @@ -871,8 +690,6 @@ class CapabilitiesFabric : public rclcpp::Node */ void trigger_first_node(const std::shared_ptr goal_handle) { - auto feedback = std::make_shared(); - auto request_trigger = std::make_shared(); std::string parameter_string; @@ -882,31 +699,65 @@ class CapabilitiesFabric : public rclcpp::Node // send the request auto result_future = - trigger_capability_client_->async_send_request(request_trigger, [this, goal_handle, feedback](TriggerCapabilityClient::SharedFuture future) { + trig_capability_client_->async_send_request(request_trigger, [this, goal_handle](TriggerCapabilityClient::SharedFuture future) { if (!future.valid()) { - auto result = std::make_shared(); - result->success = false; - result->message = "Failed to trigger capability " + connection_map[0].source.runner; - goal_handle->abort(result); - - RCLCPP_ERROR(this->get_logger(), result->message.c_str()); + status = "Failed to trigger capability " + connection_map[0].source.runner; + print_and_result(goal_handle, status, false); return; } auto response = future.get(); - feedback->progress = "Successfully triggered capability " + connection_map[0].source.runner; - goal_handle->publish_feedback(feedback); - RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); - - auto result = std::make_shared(); - result->success = true; - result->message = "Successfully launched capabilities2 fabric"; - goal_handle->succeed(result); - RCLCPP_INFO(this->get_logger(), result->message.c_str()); + + status = "Successfully triggered capability " + connection_map[0].source.runner; + print_and_feedback(goal_handle, status, false); + + print_and_result(goal_handle, "Successfully launched capabilities2 fabric", true); }); } + void check_service(bool wait_for_logic, const std::string& service_name) + { + while (wait_for_logic) + { + std::string failed = service_name + " not available"; + RCLCPP_ERROR(this->get_logger(), failed.c_str()); + rclcpp::shutdown(); + return; + } + + std::string success = service_name + " connected"; + RCLCPP_INFO(this->get_logger(), success.c_str()); + } + + void print_and_feedback(const std::shared_ptr goal_handle, const std::string& text, bool newline) + { + feedback_msg->progress = text; + goal_handle->publish_feedback(feedback_msg); + + if (newline) + RCLCPP_INFO(this->get_logger(), ""); + + RCLCPP_INFO(this->get_logger(), feedback_msg->progress.c_str()); + } + + void print_and_result(const std::shared_ptr goal_handle, const std::string& text, bool success) + { + result_msg->success = success; + result_msg->message = text; + + if (success) + { + goal_handle->succeed(result_msg); + RCLCPP_INFO(this->get_logger(), result_msg->message.c_str()); + } + else + { + goal_handle->abort(result_msg); + RCLCPP_ERROR(this->get_logger(), result_msg->message.c_str()); + } + } + private: /** File Path link */ std::string plan_file_path; @@ -926,6 +777,9 @@ class CapabilitiesFabric : public rclcpp::Node int expected_configurations_; int completed_configurations_; + /** status message string */ + std::string status; + /** Bond id */ std::string bond_id_; @@ -953,6 +807,12 @@ class CapabilitiesFabric : public rclcpp::Node /** Invalid events list */ std::vector rejected_list; + /** Feedback message for plan action server*/ + std::shared_ptr feedback_msg; + + /** Result message for plan action server*/ + std::shared_ptr result_msg; + /** action server that exposes executor*/ std::shared_ptr> planner_server_; @@ -978,8 +838,8 @@ class CapabilitiesFabric : public rclcpp::Node FreeCapabilityClient::SharedPtr free_capability_client_; /** configure an selected capability */ - ConfigureCapabilityClient::SharedPtr configure_capability_client_; + ConfigureCapabilityClient::SharedPtr conf_capability_client_; /** trigger an selected capability */ - TriggerCapabilityClient::SharedPtr trigger_capability_client_; + TriggerCapabilityClient::SharedPtr trig_capability_client_; }; \ No newline at end of file diff --git a/capabilities2_fabric/include/capabilities2_fabric/capabilities_xml_parser.hpp b/capabilities2_fabric/include/capabilities2_fabric/capabilities_xml_parser.hpp index 38ff3d3..6910d60 100644 --- a/capabilities2_fabric/include/capabilities2_fabric/capabilities_xml_parser.hpp +++ b/capabilities2_fabric/include/capabilities2_fabric/capabilities_xml_parser.hpp @@ -2,7 +2,7 @@ #include #include #include -#include +#include namespace capabilities2_xml_parser { diff --git a/capabilities2_fabric/include/capabilities2_fabric/structs/bond_client.hpp b/capabilities2_fabric/include/capabilities2_fabric/utils/bond_client.hpp similarity index 100% rename from capabilities2_fabric/include/capabilities2_fabric/structs/bond_client.hpp rename to capabilities2_fabric/include/capabilities2_fabric/utils/bond_client.hpp diff --git a/capabilities2_fabric/include/capabilities2_fabric/structs/connection.hpp b/capabilities2_fabric/include/capabilities2_fabric/utils/connection.hpp similarity index 100% rename from capabilities2_fabric/include/capabilities2_fabric/structs/connection.hpp rename to capabilities2_fabric/include/capabilities2_fabric/utils/connection.hpp From 6297a73498dce8952ff4a635137ae5ab8d221130 Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Wed, 15 Jan 2025 01:29:51 +1100 Subject: [PATCH 072/133] minor fixes --- .../launch/server.launch.py | 32 +++++++++++ capabilities2_launch_py/package.xml | 2 + capabilities2_launch_py/setup.py | 3 ++ .../capabilities2_runner/action_runner.hpp | 54 +++++++++++-------- .../capabilities2_runner/runner_base.hpp | 35 ++++++++++-- .../waypoint_runner.hpp | 7 +-- capabilities2_server/launch/server.launch.py | 14 ++--- .../launch/server_composed.launch.py | 10 +--- 8 files changed, 110 insertions(+), 47 deletions(-) create mode 100644 capabilities2_launch_py/launch/server.launch.py diff --git a/capabilities2_launch_py/launch/server.launch.py b/capabilities2_launch_py/launch/server.launch.py new file mode 100644 index 0000000..2576493 --- /dev/null +++ b/capabilities2_launch_py/launch/server.launch.py @@ -0,0 +1,32 @@ +''' +capabilities2_server launch file +''' + +import os +from launch import LaunchDescription +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + """Generate launch description for capabilities2 server + + Returns: + LaunchDescription: The launch description for capabilities2 server + """ + # load config file + server_config = os.path.join(get_package_share_directory('capabilities2_server'), 'config', 'capabilities.yaml') + + # create launch proxy node + launch_proxy = Node( + package='capabilities2_launch_py', + executable='capabilities2_launch_py', + name='capabilities2_launch_py', + output='screen', + arguments=['--ros-args', '--log-level', 'info'] + ) + + # return + return LaunchDescription([ + launch_proxy + ]) diff --git a/capabilities2_launch_py/package.xml b/capabilities2_launch_py/package.xml index 5660d5d..6c1c013 100644 --- a/capabilities2_launch_py/package.xml +++ b/capabilities2_launch_py/package.xml @@ -10,6 +10,8 @@ rclcpp capabilities2_msgs + ros2launch + ament_copyright ament_flake8 ament_pep257 diff --git a/capabilities2_launch_py/setup.py b/capabilities2_launch_py/setup.py index 717f564..605a0cf 100644 --- a/capabilities2_launch_py/setup.py +++ b/capabilities2_launch_py/setup.py @@ -1,4 +1,6 @@ from setuptools import find_packages, setup +import os +from glob import glob package_name = 'capabilities2_launch_py' @@ -9,6 +11,7 @@ data_files=[ ('share/ament_index/resource_index/packages', ['resource/' + package_name]), ('share/' + package_name, ['package.xml']), + (os.path.join('share', package_name, 'launch'), glob(os.path.join('launch', '*launch.[pxy][yma]*'))), ], install_requires=['setuptools'], zip_safe=True, diff --git a/capabilities2_runner/include/capabilities2_runner/action_runner.hpp b/capabilities2_runner/include/capabilities2_runner/action_runner.hpp index f308821..bb0b0ee 100644 --- a/capabilities2_runner/include/capabilities2_runner/action_runner.hpp +++ b/capabilities2_runner/include/capabilities2_runner/action_runner.hpp @@ -113,26 +113,33 @@ class ActionRunner : public RunnerBase */ virtual void execution() override { - execute_id += 1; + is_running = true; - // if parameters are not provided then cannot proceed - if (!parameters_) - throw runner_exception("cannot trigger action without parameters"); + while (new_parameter) + { + new_parameter = false; + + execute_id += 1; - // generate a goal from parameters if provided - goal_msg_ = generate_goal(parameters_); - RCLCPP_INFO(node_->get_logger(), "[%s] goal generated.", run_config_.interface.c_str()); + // if parameters are not provided then cannot proceed + if (!parameters_) + throw runner_exception("cannot trigger action without parameters"); - // trigger the action client with goal - send_goal_options_.goal_response_callback = - std::bind(&ActionRunner::goalResponseCallback, this, std::placeholders::_1); - send_goal_options_.feedback_callback = - std::bind(&ActionRunner::feedbackCallback, this, std::placeholders::_1, std::placeholders::_2); - send_goal_options_.result_callback = std::bind(&ActionRunner::resultCallback, this, std::placeholders::_1); + // generate a goal from parameters if provided + goal_msg_ = generate_goal(parameters_); + RCLCPP_INFO(node_->get_logger(), "[%s] goal generated.", run_config_.interface.c_str()); - goal_handle_future_ = action_client_->async_send_goal(goal_msg_, send_goal_options_); + // trigger the action client with goal + send_goal_options_.goal_response_callback = + std::bind(&ActionRunner::goalResponseCallback, this, std::placeholders::_1); + send_goal_options_.feedback_callback = + std::bind(&ActionRunner::feedbackCallback, this, std::placeholders::_1, std::placeholders::_2); + send_goal_options_.result_callback = std::bind(&ActionRunner::resultCallback, this, std::placeholders::_1); - RCLCPP_INFO(node_->get_logger(), "[%s] goal sent. Waiting for acceptance", run_config_.interface.c_str()); + goal_handle_future_ = action_client_->async_send_goal(goal_msg_, send_goal_options_); + + RCLCPP_INFO(node_->get_logger(), "[%s] goal sent. Waiting for acceptance", run_config_.interface.c_str()); + } } protected: @@ -146,7 +153,7 @@ class ActionRunner : public RunnerBase if (events[execute_id].on_started != "") { RCLCPP_INFO(node_->get_logger(), "[%s] on_started event available. Triggering", run_config_.interface.c_str()); - // triggerFunction_(events[execute_id].on_started, update_on_started(events[execute_id].on_started_param)); + triggerFunction_(events[execute_id].on_started, update_on_started(events[execute_id].on_started_param)); } } else @@ -170,7 +177,7 @@ class ActionRunner : public RunnerBase if (events[execute_id].on_success != "") { RCLCPP_INFO(node_->get_logger(), "[%s] on_success event available. Triggering", run_config_.interface.c_str()); - // triggerFunction_(events[execute_id].on_success, update_on_success(events[execute_id].on_success_param)); + triggerFunction_(events[execute_id].on_success, update_on_success(events[execute_id].on_success_param)); } } else @@ -183,11 +190,12 @@ class ActionRunner : public RunnerBase if (events[execute_id].on_failure != "") { RCLCPP_INFO(node_->get_logger(), "[%s] on_failure event available. Triggering", run_config_.interface.c_str()); - // triggerFunction_(events[execute_id].on_failure, update_on_failure(events[execute_id].on_failure_param)); + triggerFunction_(events[execute_id].on_failure, update_on_failure(events[execute_id].on_failure_param)); } } result_ = wrapped_result.result; + is_running = false; }; virtual void feedbackCallback(typename rclcpp_action::ClientGoalHandle::SharedPtr goal_handle, @@ -195,7 +203,10 @@ class ActionRunner : public RunnerBase { std::string feedback = generate_feedback(feedback_msg); - RCLCPP_INFO(node_->get_logger(), "[%s] received feedback: %s", run_config_.interface.c_str(), feedback.c_str()); + if (feedback != "") + { + RCLCPP_INFO(node_->get_logger(), "[%s] received feedback: %s", run_config_.interface.c_str(), feedback.c_str()); + } }; virtual void cancellationCallback(action_msgs::srv::CancelGoal_Response::SharedPtr response) @@ -210,7 +221,7 @@ class ActionRunner : public RunnerBase if (events[execute_id].on_stopped != "") { RCLCPP_INFO(node_->get_logger(), "[%s] on_stopped event available. Triggering", run_config_.interface.c_str()); - // triggerFunction_(events[execute_id].on_stopped, update_on_stopped(events[execute_id].on_stopped_param)); + triggerFunction_(events[execute_id].on_stopped, update_on_stopped(events[execute_id].on_stopped_param)); } } @@ -232,7 +243,8 @@ class ActionRunner : public RunnerBase * * This function is used to convert feedback messages into generic strings * - * A pattern needs to be implemented in the derived class + * A pattern needs to be implemented in the derived class. If the feedback string + * is empty, nothing will be printed on the screen * * @param parameters * @return ActionT::Feedback the received feedback diff --git a/capabilities2_runner/include/capabilities2_runner/runner_base.hpp b/capabilities2_runner/include/capabilities2_runner/runner_base.hpp index e48cea1..164f161 100644 --- a/capabilities2_runner/include/capabilities2_runner/runner_base.hpp +++ b/capabilities2_runner/include/capabilities2_runner/runner_base.hpp @@ -3,6 +3,7 @@ #include #include #include +#include #include #include #include @@ -124,7 +125,22 @@ class RunnerBase { parameters_ = convert_to_xml(parameters); - executionThread = std::thread(&RunnerBase::execution, this); + RCLCPP_INFO(node_->get_logger(), "[%s] received new parameters", run_config_.interface.c_str()); + + new_parameter = true; + + if (!is_running) + { + RCLCPP_INFO(node_->get_logger(), "[%s] execution thread not active. Activating..", + run_config_.interface.c_str()); + + executionThread = std::thread(&RunnerBase::execution, this); + } + else + { + RCLCPP_INFO(node_->get_logger(), "[%s] execution thread running. Avoiding reactivation", + run_config_.interface.c_str()); + } } /** @@ -140,6 +156,8 @@ class RunnerBase run_config_ = run_config; insert_id = 0; execute_id = -1; + is_running = false; + new_parameter = false; } /** @@ -472,15 +490,14 @@ class RunnerBase std::map events; /** - * @brief Last tracker id to be inserted + * @brief Last event tracker id to be inserted */ int insert_id; /** - * @brief Last tracker id to be executed + * @brief Last parameter tracker id to be executed */ int execute_id; - /** * @brief pointer to XMLElement which contain parameters */ @@ -491,6 +508,16 @@ class RunnerBase */ std::thread executionThread; + /** + * @brief boolean flag for status of the thread. + */ + bool is_running; + + /** + * @brief boolean flag for new parameter availability. + */ + bool new_parameter; + /** * @brief external function that triggers capability runners */ 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 ebbf9b3..56ebc49 100644 --- a/capabilities2_runner_nav2/include/capabilities2_runner_nav2/waypoint_runner.hpp +++ b/capabilities2_runner_nav2/include/capabilities2_runner_nav2/waypoint_runner.hpp @@ -78,9 +78,10 @@ class WayPointRunner : public ActionRunner virtual std::string generate_feedback(const typename nav2_msgs::action::NavigateToPose::Feedback::ConstSharedPtr msg) override { - std::string feedback = "x: " + std::to_string(msg->current_pose.pose.position.x) + - " y: " + std::to_string(msg->current_pose.pose.position.y); - return feedback; + // std::string feedback = "x: " + std::to_string(msg->current_pose.pose.position.x) + + // " y: " + std::to_string(msg->current_pose.pose.position.y); + // return feedback; + return ""; } protected: diff --git a/capabilities2_server/launch/server.launch.py b/capabilities2_server/launch/server.launch.py index 6c63d94..6879e11 100644 --- a/capabilities2_server/launch/server.launch.py +++ b/capabilities2_server/launch/server.launch.py @@ -22,18 +22,12 @@ def generate_launch_description(): package='capabilities2_server', executable='capabilities2_server_node', name='capabilities', - parameters=[server_config] - ) - - # create launch proxy node - launch_proxy = Node( - package='capabilities2_launch_py', - executable='capabilities2_launch_py', - name='capabilities2_launch_py' + parameters=[server_config], + output='screen', + arguments=['--ros-args', '--log-level', 'info'] ) # return return LaunchDescription([ - capabilities2, - launch_proxy + capabilities2 ]) diff --git a/capabilities2_server/launch/server_composed.launch.py b/capabilities2_server/launch/server_composed.launch.py index a7b4fa8..498f824 100644 --- a/capabilities2_server/launch/server_composed.launch.py +++ b/capabilities2_server/launch/server_composed.launch.py @@ -37,15 +37,7 @@ def generate_launch_description(): ] ) - # create launch proxy node - launch_proxy = Node( - package='capabilities2_launch_py', - executable='capabilities2_launch_py', - name='capabilities2_launch_py' - ) - # return return LaunchDescription([ - capabilities, - launch_proxy + capabilities ]) From 3bd2e9d6bb49d20de0fb84394ffdf46a27f3087b Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Wed, 15 Jan 2025 14:51:19 +1100 Subject: [PATCH 073/133] completed threading implementation and tested sequential triggering --- capabilities2_fabric/plans/navigation_2.xml | 2 +- .../capabilities2_runner/action_runner.hpp | 222 +++++++++--------- .../capabilities2_runner/launch_runner.hpp | 2 +- .../capabilities2_runner/runner_base.hpp | 47 ++-- .../capabilities2_runner/service_runner.hpp | 6 +- .../capabilities2_runner/topic_runner.hpp | 4 +- capabilities2_runner/src/standard_runners.cpp | 2 +- .../listen_runner.hpp | 2 - .../speak_runner.hpp | 4 +- .../bt_factory_runner.hpp | 8 +- .../waypoint_runner.hpp | 6 +- .../prompt_occupancy_runner.hpp | 2 - .../prompt_plan_request_runner.hpp | 2 - .../prompt_plan_response_runner.hpp | 2 - .../prompt_pose_runner.hpp | 2 - .../prompt_text_runner.hpp | 2 - 16 files changed, 149 insertions(+), 166 deletions(-) diff --git a/capabilities2_fabric/plans/navigation_2.xml b/capabilities2_fabric/plans/navigation_2.xml index 4b0560a..5c283d1 100644 --- a/capabilities2_fabric/plans/navigation_2.xml +++ b/capabilities2_fabric/plans/navigation_2.xml @@ -2,6 +2,6 @@ - + \ No newline at end of file diff --git a/capabilities2_runner/include/capabilities2_runner/action_runner.hpp b/capabilities2_runner/include/capabilities2_runner/action_runner.hpp index bb0b0ee..bcef201 100644 --- a/capabilities2_runner/include/capabilities2_runner/action_runner.hpp +++ b/capabilities2_runner/include/capabilities2_runner/action_runner.hpp @@ -84,7 +84,21 @@ class ActionRunner : public RunnerBase try { auto cancel_future = action_client_->async_cancel_goal( - goal_handle_, std::bind(&ActionRunner::cancellationCallback, this, std::placeholders::_1)); + 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"); + RCLCPP_WARN(node_->get_logger(), "Runner cancellation failed."); + } + + // Trigger on_stopped event if defined + if (events[execute_id].on_stopped != "") + { + RCLCPP_INFO(node_->get_logger(), "[%s] on_stopped event available. Triggering", + run_config_.interface.c_str()); + triggerFunction_(events[execute_id].on_stopped, update_on_stopped(events[execute_id].on_stopped_param)); + } + }); // wait for action to be stopped. hold the thread for 2 seconds to help keep callbacks in scope // BUG: the line below does not work in jazzy build, so a workaround is used @@ -111,120 +125,106 @@ class ActionRunner : public RunnerBase * * @param parameters pointer to tinyxml2::XMLElement that contains parameters */ - virtual void execution() override + virtual void execution(int id) override { - is_running = true; - - while (new_parameter) - { - new_parameter = false; - - execute_id += 1; - - // if parameters are not provided then cannot proceed - if (!parameters_) - throw runner_exception("cannot trigger action without parameters"); - - // generate a goal from parameters if provided - goal_msg_ = generate_goal(parameters_); - RCLCPP_INFO(node_->get_logger(), "[%s] goal generated.", run_config_.interface.c_str()); - - // trigger the action client with goal - send_goal_options_.goal_response_callback = - std::bind(&ActionRunner::goalResponseCallback, this, std::placeholders::_1); - send_goal_options_.feedback_callback = - std::bind(&ActionRunner::feedbackCallback, this, std::placeholders::_1, std::placeholders::_2); - send_goal_options_.result_callback = std::bind(&ActionRunner::resultCallback, this, std::placeholders::_1); - - goal_handle_future_ = action_client_->async_send_goal(goal_msg_, send_goal_options_); - - RCLCPP_INFO(node_->get_logger(), "[%s] goal sent. Waiting for acceptance", run_config_.interface.c_str()); - } + execute_id += 1; + + // if parameters are not provided then cannot proceed + if (!parameters_[id]) + throw runner_exception("cannot trigger action without parameters"); + + // generate a goal from parameters if provided + goal_msg_ = generate_goal(parameters_[id]); + + RCLCPP_INFO(node_->get_logger(), "[%s/%d] goal generated.", run_config_.interface.c_str(), id); + + std::unique_lock lock(send_goal_mutex); + action_complete = false; + + // trigger the action client with goal + send_goal_options_.goal_response_callback = + [this, id](const typename rclcpp_action::ClientGoalHandle::SharedPtr& goal_handle) { + if (goal_handle) + { + RCLCPP_INFO(node_->get_logger(), "[%s/%d] goal accepted. Waiting for result", run_config_.interface.c_str(), + id); + + // trigger the events related to on_started state + if (events[execute_id].on_started != "") + { + RCLCPP_INFO(node_->get_logger(), "[%s/%d] on_started event available. Triggering", + run_config_.interface.c_str(), id); + + triggerFunction_(events[execute_id].on_started, update_on_started(events[execute_id].on_started_param)); + } + } + else + { + RCLCPP_ERROR(node_->get_logger(), "[%s/%d] goal rejected", run_config_.interface.c_str(), id); + } + + // store goal handle to be used with stop funtion + goal_handle_ = goal_handle; + }; + + send_goal_options_.feedback_callback = + [this, id](typename rclcpp_action::ClientGoalHandle::SharedPtr goal_handle, + const typename ActionT::Feedback::ConstSharedPtr feedback_msg) { + std::string feedback = generate_feedback(feedback_msg); + + if (feedback != "") + { + RCLCPP_INFO(node_->get_logger(), "[%s/%d] received feedback: %s", run_config_.interface.c_str(), id, + feedback.c_str()); + } + }; + + send_goal_options_.result_callback = + [this, id](const typename rclcpp_action::ClientGoalHandle::WrappedResult& wrapped_result) { + RCLCPP_INFO(node_->get_logger(), "[%s/%d] received result", run_config_.interface.c_str(), id); + + if (wrapped_result.code == rclcpp_action::ResultCode::SUCCEEDED) + { + RCLCPP_INFO(node_->get_logger(), "[%s/%d] action succeeded.", run_config_.interface.c_str(), id); + + // trigger the events related to on_success state + if (events[execute_id].on_success != "") + { + RCLCPP_INFO(node_->get_logger(), "[%s/%d] on_success event available. Triggering", + run_config_.interface.c_str(), id); + triggerFunction_(events[execute_id].on_success, update_on_success(events[execute_id].on_success_param)); + } + } + else + { + RCLCPP_ERROR(node_->get_logger(), "[%s/%d] action failed.", run_config_.interface.c_str(), id); + RCLCPP_ERROR(node_->get_logger(), "[%s/%d] error code: %d", run_config_.interface.c_str(), id, + static_cast(wrapped_result.code)); + + // trigger the events related to on_failure state + if (events[execute_id].on_failure != "") + { + RCLCPP_INFO(node_->get_logger(), "[%s/%d] on_failure event available. Triggering", + run_config_.interface.c_str(), id); + triggerFunction_(events[execute_id].on_failure, update_on_failure(events[execute_id].on_failure_param)); + } + } + + result_ = wrapped_result.result; + action_complete = true; + send_goal_cv.notify_all(); + }; + + goal_handle_future_ = action_client_->async_send_goal(goal_msg_, send_goal_options_); + + RCLCPP_INFO(node_->get_logger(), "[%s/%d] goal sent. Waiting for acceptance", run_config_.interface.c_str(), id); + + // Conditional wait + send_goal_cv.wait(lock, [this] { return action_complete; }); + RCLCPP_INFO(node_->get_logger(), "[%s/%d] action complete. Thread closing.", run_config_.interface.c_str(), id); } protected: - virtual void goalResponseCallback(const typename rclcpp_action::ClientGoalHandle::SharedPtr& goal_handle) - { - if (goal_handle) - { - RCLCPP_INFO(node_->get_logger(), "[%s] goal accepted. Waiting for result", run_config_.interface.c_str()); - - // trigger the events related to on_started state - if (events[execute_id].on_started != "") - { - RCLCPP_INFO(node_->get_logger(), "[%s] on_started event available. Triggering", run_config_.interface.c_str()); - triggerFunction_(events[execute_id].on_started, update_on_started(events[execute_id].on_started_param)); - } - } - else - { - RCLCPP_ERROR(node_->get_logger(), "[%s] goal rejected", run_config_.interface.c_str()); - } - - // store goal handle to be used with stop funtion - goal_handle_ = goal_handle; - }; - - virtual void resultCallback(const typename rclcpp_action::ClientGoalHandle::WrappedResult& wrapped_result) - { - RCLCPP_INFO(node_->get_logger(), "[%s] received result", run_config_.interface.c_str()); - - if (wrapped_result.code == rclcpp_action::ResultCode::SUCCEEDED) - { - RCLCPP_INFO(node_->get_logger(), "[%s] action succeeded.", run_config_.interface.c_str()); - - // trigger the events related to on_success state - if (events[execute_id].on_success != "") - { - RCLCPP_INFO(node_->get_logger(), "[%s] on_success event available. Triggering", run_config_.interface.c_str()); - triggerFunction_(events[execute_id].on_success, update_on_success(events[execute_id].on_success_param)); - } - } - else - { - RCLCPP_ERROR(node_->get_logger(), "[%s] action failed.", run_config_.interface.c_str()); - RCLCPP_ERROR(node_->get_logger(), "[%s] error code: %d", run_config_.interface.c_str(), - static_cast(wrapped_result.code)); - - // trigger the events related to on_failure state - if (events[execute_id].on_failure != "") - { - RCLCPP_INFO(node_->get_logger(), "[%s] on_failure event available. Triggering", run_config_.interface.c_str()); - triggerFunction_(events[execute_id].on_failure, update_on_failure(events[execute_id].on_failure_param)); - } - } - - result_ = wrapped_result.result; - is_running = false; - }; - - virtual void feedbackCallback(typename rclcpp_action::ClientGoalHandle::SharedPtr goal_handle, - const typename ActionT::Feedback::ConstSharedPtr feedback_msg) - { - std::string feedback = generate_feedback(feedback_msg); - - if (feedback != "") - { - RCLCPP_INFO(node_->get_logger(), "[%s] received feedback: %s", run_config_.interface.c_str(), feedback.c_str()); - } - }; - - virtual void cancellationCallback(action_msgs::srv::CancelGoal_Response::SharedPtr response) - { - if (response->return_code != action_msgs::srv::CancelGoal_Response::ERROR_NONE) - { - // throw runner_exception("failed to cancel runner"); - RCLCPP_WARN(node_->get_logger(), "Runner cancellation failed."); - } - - // Trigger on_stopped event if defined - if (events[execute_id].on_stopped != "") - { - RCLCPP_INFO(node_->get_logger(), "[%s] on_stopped event available. Triggering", run_config_.interface.c_str()); - triggerFunction_(events[execute_id].on_stopped, update_on_stopped(events[execute_id].on_stopped_param)); - } - } - /** * @brief Generate a goal from parameters * diff --git a/capabilities2_runner/include/capabilities2_runner/launch_runner.hpp b/capabilities2_runner/include/capabilities2_runner/launch_runner.hpp index dcc8781..1b74036 100644 --- a/capabilities2_runner/include/capabilities2_runner/launch_runner.hpp +++ b/capabilities2_runner/include/capabilities2_runner/launch_runner.hpp @@ -129,7 +129,7 @@ class LaunchRunner : public RunnerBase protected: // throw on triggerExecution function - void execution() override + void execution(int id) override { throw runner_exception("no triggerExecution() this is a no-trigger action runner"); } diff --git a/capabilities2_runner/include/capabilities2_runner/runner_base.hpp b/capabilities2_runner/include/capabilities2_runner/runner_base.hpp index 164f161..2733b44 100644 --- a/capabilities2_runner/include/capabilities2_runner/runner_base.hpp +++ b/capabilities2_runner/include/capabilities2_runner/runner_base.hpp @@ -123,24 +123,15 @@ class RunnerBase */ virtual void trigger(const std::string& parameters) { - parameters_ = convert_to_xml(parameters); + RCLCPP_INFO(node_->get_logger(), "[%s/%d] received new parameters", run_config_.interface.c_str(), thread_id); - RCLCPP_INFO(node_->get_logger(), "[%s] received new parameters", run_config_.interface.c_str()); + parameters_[thread_id] = convert_to_xml(parameters); - new_parameter = true; + executionThreadPool[thread_id] = std::thread(&RunnerBase::execution, this, thread_id); - if (!is_running) - { - RCLCPP_INFO(node_->get_logger(), "[%s] execution thread not active. Activating..", - run_config_.interface.c_str()); - - executionThread = std::thread(&RunnerBase::execution, this); - } - else - { - RCLCPP_INFO(node_->get_logger(), "[%s] execution thread running. Avoiding reactivation", - run_config_.interface.c_str()); - } + RCLCPP_INFO(node_->get_logger(), "[%s/%d] started execution", run_config_.interface.c_str(), thread_id); + + thread_id += 1; } /** @@ -156,8 +147,7 @@ class RunnerBase run_config_ = run_config; insert_id = 0; execute_id = -1; - is_running = false; - new_parameter = false; + thread_id = 0; } /** @@ -230,7 +220,7 @@ class RunnerBase * @param parameters pointer to tinyxml2::XMLElement that contains parameters * */ - virtual void execution() = 0; + virtual void execution(int id) = 0; /** * @brief Update on_started event parameters with new data if avaible. @@ -498,25 +488,32 @@ class RunnerBase * @brief Last parameter tracker id to be executed */ int execute_id; + + /** + * @brief Last parameter tracker id to be executed + */ + int thread_id; + /** * @brief pointer to XMLElement which contain parameters */ - tinyxml2::XMLElement* parameters_; + std::map parameters_; /** - * @brief thread that executes the triggerExecution functionality + * @brief dictionary of threads that executes the triggerExecution functionality */ - std::thread executionThread; + std::map executionThreadPool; /** - * @brief boolean flag for status of the thread. + * @brief mutex and conditional variable for threadpool synchronisation. */ - bool is_running; + std::mutex send_goal_mutex; + std::condition_variable send_goal_cv; /** - * @brief boolean flag for new parameter availability. + * @brief boolean flag for thread completion. */ - bool new_parameter; + bool action_complete; /** * @brief external function that triggers capability runners diff --git a/capabilities2_runner/include/capabilities2_runner/service_runner.hpp b/capabilities2_runner/include/capabilities2_runner/service_runner.hpp index 498d18a..6fbf2d2 100644 --- a/capabilities2_runner/include/capabilities2_runner/service_runner.hpp +++ b/capabilities2_runner/include/capabilities2_runner/service_runner.hpp @@ -58,16 +58,16 @@ class ServiceRunner : public RunnerBase * * @param parameters pointer to tinyxml2::XMLElement that contains parameters */ - virtual void execution() override + virtual void execution(int id) override { execute_id += 1; // if parameters are not provided then cannot proceed - if (!parameters_) + if (!parameters_[id]) throw runner_exception("cannot trigger service without parameters"); // generate a goal from parameters if provided - auto request_msg = std::make_shared(generate_request(parameters_)); + auto request_msg = std::make_shared(generate_request(parameters_[id])); auto result_future = service_client_->async_send_request( request_msg, [this](typename rclcpp::Client::SharedFuture future) { diff --git a/capabilities2_runner/include/capabilities2_runner/topic_runner.hpp b/capabilities2_runner/include/capabilities2_runner/topic_runner.hpp index 26f5840..2450851 100644 --- a/capabilities2_runner/include/capabilities2_runner/topic_runner.hpp +++ b/capabilities2_runner/include/capabilities2_runner/topic_runner.hpp @@ -50,12 +50,12 @@ class TopicRunner : public RunnerBase * * @param parameters pointer to tinyxml2::XMLElement that contains parameters */ - virtual void execution() override + virtual void execution(int id) override { execute_id += 1; // if parameters are not provided then cannot proceed - if (!parameters_) + if (!parameters_[id]) throw runner_exception("cannot grab data without parameters"); // trigger the events related to on_started state diff --git a/capabilities2_runner/src/standard_runners.cpp b/capabilities2_runner/src/standard_runners.cpp index 459aa42..58ca354 100644 --- a/capabilities2_runner/src/standard_runners.cpp +++ b/capabilities2_runner/src/standard_runners.cpp @@ -33,7 +33,7 @@ class DummyRunner : public RunnerBase protected: // throw on triggerExecution function - void execution() override + void execution(int id) override { RCLCPP_INFO(node_->get_logger(), "Dummy runner does not have triggerExecution()"); } 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 35ca8d2..84fd7a7 100644 --- a/capabilities2_runner_audio/include/capabilities2_runner_audio/listen_runner.hpp +++ b/capabilities2_runner_audio/include/capabilities2_runner_audio/listen_runner.hpp @@ -46,8 +46,6 @@ class ListenerRunner : public ActionRunner */ virtual hri_audio_msgs::action::SpeechToText::Goal generate_goal(tinyxml2::XMLElement* parameters) override { - parameters_ = parameters; - hri_audio_msgs::action::SpeechToText::Goal goal_msg; goal_msg.header.stamp = node_->get_clock()->now(); diff --git a/capabilities2_runner_audio/include/capabilities2_runner_audio/speak_runner.hpp b/capabilities2_runner_audio/include/capabilities2_runner_audio/speak_runner.hpp index 7f55443..014e953 100644 --- a/capabilities2_runner_audio/include/capabilities2_runner_audio/speak_runner.hpp +++ b/capabilities2_runner_audio/include/capabilities2_runner_audio/speak_runner.hpp @@ -45,10 +45,8 @@ class SpeakerRunner : public ActionRunner */ virtual hri_audio_msgs::action::TextToSpeech::Goal generate_goal(tinyxml2::XMLElement* parameters) override { - parameters_ = parameters; - const char **text; - parameters_->QueryStringAttribute("text", text); + parameters->QueryStringAttribute("text", text); std::string tts_text(*text); hri_audio_msgs::action::TextToSpeech::Goal goal_msg; diff --git a/capabilities2_runner_bt/include/capabilities2_runner_bt/bt_factory_runner.hpp b/capabilities2_runner_bt/include/capabilities2_runner_bt/bt_factory_runner.hpp index ff3b463..846f1a8 100644 --- a/capabilities2_runner_bt/include/capabilities2_runner_bt/bt_factory_runner.hpp +++ b/capabilities2_runner_bt/include/capabilities2_runner_bt/bt_factory_runner.hpp @@ -50,14 +50,16 @@ class BTRunnerBase : public RunnerBase, public BT::BehaviorTreeFactory * @param parameters * @return std::optional)>> */ - virtual void trigger(tinyxml2::XMLElement* parameters = nullptr) override + virtual void trigger(const std::string& parameters) override { + tinyxml2::XMLElement* parameters_xml = convert_to_xml(parameters); + // if parameters are not provided then cannot proceed - if (!parameters) + if (!parameters_xml) throw runner_exception("cannot trigger action without parameters"); // create the tree (ptr) - tree_ = std::make_shared(this->createTreeFromText(parameters->GetText())); + tree_ = std::make_shared(this->createTreeFromText(parameters_xml->GetText())); // return the tick function // // the caller can call this function to tick the tree 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 56ebc49..0a5cd6d 100644 --- a/capabilities2_runner_nav2/include/capabilities2_runner_nav2/waypoint_runner.hpp +++ b/capabilities2_runner_nav2/include/capabilities2_runner_nav2/waypoint_runner.hpp @@ -48,10 +48,8 @@ class WayPointRunner : public ActionRunner */ virtual nav2_msgs::action::NavigateToPose::Goal generate_goal(tinyxml2::XMLElement* parameters) override { - parameters_ = parameters; - - parameters_->QueryDoubleAttribute("x", &x); - parameters_->QueryDoubleAttribute("y", &y); + parameters->QueryDoubleAttribute("x", &x); + parameters->QueryDoubleAttribute("y", &y); RCLCPP_INFO(node_->get_logger(), "[%s] goal consist of x: %f and y: %f", run_config_.interface.c_str(), x, y); diff --git a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_occupancy_runner.hpp b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_occupancy_runner.hpp index 709f90d..cea6c6d 100644 --- a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_occupancy_runner.hpp +++ b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_occupancy_runner.hpp @@ -46,8 +46,6 @@ class PromptOccupancyRunner : public ServiceRunner */ virtual typename prompt_msgs::srv::Prompt::Request generate_request(tinyxml2::XMLElement* parameters) override { - parameters_ = parameters; - tinyxml2::XMLElement* occupancyElement = parameters->FirstChildElement("OccupancyGrid"); tinyxml2::XMLPrinter printer; diff --git a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_request_runner.hpp b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_request_runner.hpp index 7e17207..b75c5e5 100644 --- a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_request_runner.hpp +++ b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_request_runner.hpp @@ -47,8 +47,6 @@ class PromptPlanRequestRunner : public ServiceRunner */ virtual typename prompt_msgs::srv::Prompt::Request generate_request(tinyxml2::XMLElement* parameters) override { - parameters_ = parameters; - bool replan; parameters->QueryBoolAttribute("replan", &replan); diff --git a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_response_runner.hpp b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_response_runner.hpp index 67a5c31..d6c3a36 100644 --- a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_response_runner.hpp +++ b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_response_runner.hpp @@ -45,8 +45,6 @@ class PromptPlanResponseRunner : public ActionRunnerFirstChildElement("ReceievdPlan"); auto goal_msg = capabilities2_msgs::action::Plan::Goal(); diff --git a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_pose_runner.hpp b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_pose_runner.hpp index 1205541..3dda94a 100644 --- a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_pose_runner.hpp +++ b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_pose_runner.hpp @@ -46,8 +46,6 @@ class PromptPoseRunner : public ServiceRunner */ virtual typename prompt_msgs::srv::Prompt::Request generate_request(tinyxml2::XMLElement* parameters) override { - parameters_ = parameters; - tinyxml2::XMLElement* poseElement = parameters->FirstChildElement("Pose"); tinyxml2::XMLPrinter printer; diff --git a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_text_runner.hpp b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_text_runner.hpp index 0058a85..a418f76 100644 --- a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_text_runner.hpp +++ b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_text_runner.hpp @@ -46,8 +46,6 @@ class PromptTextRunner : public ServiceRunner */ virtual typename prompt_msgs::srv::Prompt::Request generate_request(tinyxml2::XMLElement* parameters) override { - parameters_ = parameters; - tinyxml2::XMLElement* textElement = parameters->FirstChildElement("Text"); tinyxml2::XMLPrinter printer; From 541895b747abc3d5c9e8c22bd6d7df97606e3247 Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Thu, 16 Jan 2025 14:18:26 +1100 Subject: [PATCH 074/133] completed testing of thread based fabric triggering using nav2 stack --- .../capabilities2_launch}/__init__.py | 0 .../capabilities2_launch.py | 146 ++++++++++++++++++ .../launch/server.launch.py | 19 ++- .../launch_server}/__init__.py | 0 .../launch_server}/launch_process.py | 47 +++++- capabilities2_launch/launch_server/server.py | 65 ++++++++ .../package.xml | 4 +- .../resource/capabilities2_launch | 0 capabilities2_launch/setup.cfg | 4 + .../setup.py | 5 +- .../test/test_copyright.py | 0 .../test/test_flake8.py | 0 .../test/test_pep257.py | 0 .../capabilities2_launch_py/launch_server.py | 79 ---------- capabilities2_launch_py/setup.cfg | 4 - capabilities2_msgs/srv/Launch.srv | 3 +- .../capabilities2_runner/launch_runner.hpp | 20 +-- capabilities2_runner_nav2/CMakeLists.txt | 2 + .../waypoint_runner.hpp | 8 + 19 files changed, 300 insertions(+), 106 deletions(-) rename {capabilities2_launch_py/capabilities2_launch_py => capabilities2_launch/capabilities2_launch}/__init__.py (100%) create mode 100644 capabilities2_launch/capabilities2_launch/capabilities2_launch.py rename {capabilities2_launch_py => capabilities2_launch}/launch/server.launch.py (51%) rename {capabilities2_launch_py/launch_process => capabilities2_launch/launch_server}/__init__.py (100%) rename {capabilities2_launch_py/launch_process => capabilities2_launch/launch_server}/launch_process.py (69%) create mode 100644 capabilities2_launch/launch_server/server.py rename {capabilities2_launch_py => capabilities2_launch}/package.xml (94%) rename capabilities2_launch_py/resource/capabilities2_launch_py => capabilities2_launch/resource/capabilities2_launch (100%) create mode 100644 capabilities2_launch/setup.cfg rename {capabilities2_launch_py => capabilities2_launch}/setup.py (77%) rename {capabilities2_launch_py => capabilities2_launch}/test/test_copyright.py (100%) rename {capabilities2_launch_py => capabilities2_launch}/test/test_flake8.py (100%) rename {capabilities2_launch_py => capabilities2_launch}/test/test_pep257.py (100%) delete mode 100644 capabilities2_launch_py/capabilities2_launch_py/launch_server.py delete mode 100644 capabilities2_launch_py/setup.cfg diff --git a/capabilities2_launch_py/capabilities2_launch_py/__init__.py b/capabilities2_launch/capabilities2_launch/__init__.py similarity index 100% rename from capabilities2_launch_py/capabilities2_launch_py/__init__.py rename to capabilities2_launch/capabilities2_launch/__init__.py diff --git a/capabilities2_launch/capabilities2_launch/capabilities2_launch.py b/capabilities2_launch/capabilities2_launch/capabilities2_launch.py new file mode 100644 index 0000000..854c232 --- /dev/null +++ b/capabilities2_launch/capabilities2_launch/capabilities2_launch.py @@ -0,0 +1,146 @@ +import os +import signal +import rclpy +from rclpy.node import Node +from capabilities2_msgs.srv import Launch +from multiprocessing.connection import Client + +class LaunchServer(Node): + def __init__(self): + super().__init__('capabilities2_launch_server') + self.address = ('localhost', 6000) + self.client = Client(self.address, authkey=b'capabilities2') + + # Service for starting a launch file + self.start_server = self.create_service( + Launch, + '/capabilities/launch/start', + self.start_request + ) + + # Service for stopping a launch file + self.stop_server = self.create_service( + Launch, + '/capabilities/launch/stop', + self.stop_request + ) + + # Service for stopping a launch file + self.stop_server = self.create_service( + Launch, + '/capabilities/launch/status', + self.status_request + ) + + self.get_logger().info('Capabilities2 LaunchServer is ready.') + + def start_request(self, request, response): + package_name = request.package_name + launch_file_name = request.launch_file_name + + self.get_logger().info(f'[{package_name}/{launch_file_name}] received start request') + + message_outgoing = {} + message_outgoing["command"] = "start" + message_outgoing["package"] = package_name + message_outgoing["launch_file"] = launch_file_name + + self.get_logger().info(f'[{package_name}/{launch_file_name}] sending start request to Launch Server') + + self.client.send(message_outgoing) + + self.get_logger().info(f'[{package_name}/{launch_file_name}] waiting for response from Launch Server') + + message_incoming = self.client.recv() + response_content = message_incoming["status"] + + self.get_logger().info(f'[{package_name}/{launch_file_name}] received response : {response_content}') + + response.status = response_content + + return response + + def stop_request(self, request, response): + package_name = request.package_name + launch_file_name = request.launch_file_name + + self.get_logger().info(f'[{package_name}/{launch_file_name}] received stop request') + + message_outgoing = {} + message_outgoing["command"] = "stop" + message_outgoing["package"] = package_name + message_outgoing["launch_file"] = launch_file_name + + self.get_logger().info(f'[{package_name}/{launch_file_name}] sending stop request to Launch Server') + + self.client.send(message_outgoing) + + self.get_logger().info(f'[{package_name}/{launch_file_name}] waiting for response from Launch Server') + + message_incoming = self.client.recv() + response_content = message_incoming["status"] + + self.get_logger().info(f'[{package_name}/{launch_file_name}] received response : {response_content}') + + response.status = response_content + + return response + + + def status_request(self, request, response): + package_name = request.package_name + launch_file_name = request.launch_file_name + + self.get_logger().info(f'[{package_name}/{launch_file_name}] received status request') + + message_outgoing = {} + message_outgoing["command"] = "status" + message_outgoing["package"] = package_name + message_outgoing["launch_file"] = launch_file_name + + self.get_logger().info(f'[{package_name}/{launch_file_name}] sending status request to Launch Server') + + self.client.send(message_outgoing) + + self.get_logger().info(f'[{package_name}/{launch_file_name}] waiting for response from Launch Server') + + message_incoming = self.client.recv() + response_content = message_incoming["status"] + + self.get_logger().info(f'[{package_name}/{launch_file_name}] received response : {response_content}') + + response.status = response_content + + return response + + def shutdown(self): + + self.get_logger().info(f'Shutting down LaunchFileServer...') + + message_outgoing = {} + message_outgoing["command"] = "exit" + message_outgoing["package"] = "" + message_outgoing["launch_file"] = "" + + self.get_logger().info(f'[sending shutdown request to Launch Server') + + self.client.send(message_outgoing) + + +def main(args=None): + rclpy.init(args=args) + + # Create the LaunchServer node + launch_server = LaunchServer() + + # Keep the node running + try: + rclpy.spin(launch_server) + except KeyboardInterrupt: + launch_server.shutdown() + finally: + rclpy.shutdown() + + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/capabilities2_launch_py/launch/server.launch.py b/capabilities2_launch/launch/server.launch.py similarity index 51% rename from capabilities2_launch_py/launch/server.launch.py rename to capabilities2_launch/launch/server.launch.py index 2576493..8ce3c51 100644 --- a/capabilities2_launch_py/launch/server.launch.py +++ b/capabilities2_launch/launch/server.launch.py @@ -4,7 +4,9 @@ import os from launch import LaunchDescription +from launch.actions import ExecuteProcess from launch_ros.actions import Node +from launch.substitutions import FindExecutable from ament_index_python.packages import get_package_share_directory @@ -14,19 +16,22 @@ def generate_launch_description(): Returns: LaunchDescription: The launch description for capabilities2 server """ - # load config file - server_config = os.path.join(get_package_share_directory('capabilities2_server'), 'config', 'capabilities.yaml') + + filePath = os.path.join(get_package_share_directory('capabilities2_launch'), 'launch_server', 'server.py') # create launch proxy node - launch_proxy = Node( - package='capabilities2_launch_py', - executable='capabilities2_launch_py', - name='capabilities2_launch_py', + launch_interface = Node( + package='capabilities2_launch', + executable='capabilities2_launch', + name='capabilities2_launch', output='screen', arguments=['--ros-args', '--log-level', 'info'] ) + server_process = ExecuteProcess(cmd=[[FindExecutable(name='python3'), ' ', filePath]], shell=True, output='screen') + # return return LaunchDescription([ - launch_proxy + launch_interface, + server_process ]) diff --git a/capabilities2_launch_py/launch_process/__init__.py b/capabilities2_launch/launch_server/__init__.py similarity index 100% rename from capabilities2_launch_py/launch_process/__init__.py rename to capabilities2_launch/launch_server/__init__.py diff --git a/capabilities2_launch_py/launch_process/launch_process.py b/capabilities2_launch/launch_server/launch_process.py similarity index 69% rename from capabilities2_launch_py/launch_process/launch_process.py rename to capabilities2_launch/launch_server/launch_process.py index 14ba22d..04af439 100644 --- a/capabilities2_launch_py/launch_process/launch_process.py +++ b/capabilities2_launch/launch_server/launch_process.py @@ -49,7 +49,7 @@ def __init__( raise RuntimeError(str(exc)) def run(self): - print(f"Launch file {self.launch_file_name} from package {self.package_name} started.") + print(f"Launch file {self.launch_file_name} from package {self.package_name} started with pid [{self.pid}") launch_service = launch.LaunchService( argv=self.launch_file_arguments, @@ -79,6 +79,51 @@ def stop(self): print(f"Stopping Launch file {self.launch_file_name} from package {self.package_name}") self.kill() +class LaunchManager: + def __init__(self): + self.processes = {} + + def start(self, package_name, launch_file_name): + name = package_name + "/" + launch_file_name + + if name not in self.processes: + try: + self.processes[name] = LaunchProcess(package_name=package_name, launch_file_name=launch_file_name) + self.processes[name].start() + except: + return {"status": "error occured"} + else: + return {"status": "successfuly started"} + else: + return {"status": "already started. ignoring"} + + def status(self, package_name, launch_file_name): + name = package_name + "/" + launch_file_name + + if name in self.processes: + if self.processes[name].is_alive(): + return {"status": "running"} + else: + return {"status": "failed"} + else: + return {"status": "stopped"} + + + def stop(self, package_name, launch_file_name): + name = package_name + "/" + launch_file_name + + if name in self.processes: + try: + self.processes[name].stop() + self.processes[name].join() + except: + return {"status": "error occured"} + else: + return {"status": "successfuly stopped"} + else: + return {"status": "already stopped. ignoring"} + + if __name__ == "__main__": launch_process = LaunchProcess( package_name="nav_stack", diff --git a/capabilities2_launch/launch_server/server.py b/capabilities2_launch/launch_server/server.py new file mode 100644 index 0000000..beb0251 --- /dev/null +++ b/capabilities2_launch/launch_server/server.py @@ -0,0 +1,65 @@ +from launch_server.launch_process import LaunchManager +from multiprocessing.connection import Listener + +class LaunchServer: + def __init__(self): + self.address = ('localhost', 6000) + self.listener = Listener(self.address, authkey=b'capabilities2') + self.manager = LaunchManager() + + def initialize(self): + self.connection = self.listener.accept() + print("[Launch Server] connection Accepted from {self.listener.last_accepted}") + + def run(self): + while True: + # message would be a dictionary with the format of {"command": , "package": , "launch_file": } + message_incoming = self.connection.recv() + + command = message_incoming["command"] + + if ((command=="start") or (command=="stop") or (command=="status")): + package_name = message_incoming["package"] + launch_file_name = message_incoming["launch_file"] + else: + package_name = "" + launch_file_name = "" + + if (command=="start"): + print("[Launch Server] start request for {package_name}/{launch_file_name}") + status = self.manager.start(package_name=package_name, launch_file_name=launch_file_name) + result = status["status"] + + print("[Launch Server] start response of {package_name}/{launch_file_name}: {result}") + + elif (command=="stop"): + print("[Launch Server] stop request for {package_name}/{launch_file_name}") + status = self.manager.stop(package_name=package_name, launch_file_name=launch_file_name) + result = status["status"] + + print("[Launch Server] stop response of {package_name}/{launch_file_name}: {result}") + + elif (command=="status"): + print("[Launch Server] status request for {package_name}/{launch_file_name}") + status = self.manager.status(package_name=package_name, launch_file_name=launch_file_name) + result = status["status"] + + print("[Launch Server] status response of {package_name}/{launch_file_name}: {result}") + + elif (command=="exit"): + break + + else: + print("[Launch Server] does not support command of type : {command}") + break + + self.connection.send(status) + + self.listener.close + + +if __name__ == "__main__": + + server = LaunchServer() + server.initialize() + server.run() \ No newline at end of file diff --git a/capabilities2_launch_py/package.xml b/capabilities2_launch/package.xml similarity index 94% rename from capabilities2_launch_py/package.xml rename to capabilities2_launch/package.xml index 6c1c013..8b0c6fa 100644 --- a/capabilities2_launch_py/package.xml +++ b/capabilities2_launch/package.xml @@ -1,7 +1,7 @@ - capabilities2_launch_py + capabilities2_launch 0.0.0 TODO: Package description kalana @@ -11,7 +11,7 @@ capabilities2_msgs ros2launch - + ament_copyright ament_flake8 ament_pep257 diff --git a/capabilities2_launch_py/resource/capabilities2_launch_py b/capabilities2_launch/resource/capabilities2_launch similarity index 100% rename from capabilities2_launch_py/resource/capabilities2_launch_py rename to capabilities2_launch/resource/capabilities2_launch diff --git a/capabilities2_launch/setup.cfg b/capabilities2_launch/setup.cfg new file mode 100644 index 0000000..9844c9c --- /dev/null +++ b/capabilities2_launch/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/capabilities2_launch +[install] +install_scripts=$base/lib/capabilities2_launch diff --git a/capabilities2_launch_py/setup.py b/capabilities2_launch/setup.py similarity index 77% rename from capabilities2_launch_py/setup.py rename to capabilities2_launch/setup.py index 605a0cf..856147b 100644 --- a/capabilities2_launch_py/setup.py +++ b/capabilities2_launch/setup.py @@ -2,7 +2,7 @@ import os from glob import glob -package_name = 'capabilities2_launch_py' +package_name = 'capabilities2_launch' setup( name=package_name, @@ -12,6 +12,7 @@ ('share/ament_index/resource_index/packages', ['resource/' + package_name]), ('share/' + package_name, ['package.xml']), (os.path.join('share', package_name, 'launch'), glob(os.path.join('launch', '*launch.[pxy][yma]*'))), + (os.path.join('share', package_name, 'launch_server'), glob(os.path.join('launch_server', '*.py'))), ], install_requires=['setuptools'], zip_safe=True, @@ -22,7 +23,7 @@ tests_require=['pytest'], entry_points={ 'console_scripts': [ - 'capabilities2_launch_py = capabilities2_launch_py.launch_server:main', + 'capabilities2_launch = capabilities2_launch.capabilities2_launch:main', ], }, ) diff --git a/capabilities2_launch_py/test/test_copyright.py b/capabilities2_launch/test/test_copyright.py similarity index 100% rename from capabilities2_launch_py/test/test_copyright.py rename to capabilities2_launch/test/test_copyright.py diff --git a/capabilities2_launch_py/test/test_flake8.py b/capabilities2_launch/test/test_flake8.py similarity index 100% rename from capabilities2_launch_py/test/test_flake8.py rename to capabilities2_launch/test/test_flake8.py diff --git a/capabilities2_launch_py/test/test_pep257.py b/capabilities2_launch/test/test_pep257.py similarity index 100% rename from capabilities2_launch_py/test/test_pep257.py rename to capabilities2_launch/test/test_pep257.py diff --git a/capabilities2_launch_py/capabilities2_launch_py/launch_server.py b/capabilities2_launch_py/capabilities2_launch_py/launch_server.py deleted file mode 100644 index 0f1a462..0000000 --- a/capabilities2_launch_py/capabilities2_launch_py/launch_server.py +++ /dev/null @@ -1,79 +0,0 @@ -import os -import signal -import rclpy -from launch_process import launch_process -from rclpy.node import Node -from capabilities2_msgs.srv import Launch - -class LaunchServer(Node): - def __init__(self): - super().__init__('launch_file_server') - - # Service for starting a launch file - self.start_server = self.create_service( - Launch, - '/capabilities/launch_start', - self.start_request - ) - - # Service for stopping a launch file - self.stop_server = self.create_service( - Launch, - '/capabilities/launch_stop', - self.stop_request - ) - - # Dictionary to track running processes - self.processes = {} - - self.get_logger().info('LaunchFileServer is ready.') - - def start_request(self, request, response): - package_name = request.package_name - launch_name = request.launch_file_name - - self.get_logger().info(f'Received launch start request: package = {package_name}, launch file = {launch_name}') - - name = package_name + "/" + launch_name - - self.processes[name] = launch_process.LaunchProcess(package_name=package_name, launch_file_name=launch_name) - self.processes[name].start() - - return response - - def stop_request(self, request, response): - package_name = request.package_name - launch_name = request.launch_file_name - - self.get_logger().info(f'Received launch stop request: package = {package_name}, launch file = {launch_name}') - - name = package_name + "/" + launch_name - - self.processes[name].stop() - self.processes[name].join() - - return response - - -def main(args=None): - rclpy.init(args=args) - - # Create the LaunchServer node - launch_server = LaunchServer() - - # Keep the node running - try: - rclpy.spin(launch_server) - except KeyboardInterrupt: - launch_server.get_logger().info('Shutting down LaunchFileServer...') - finally: - # Clean up all running processes - for pid, process in launch_server.processes.items(): - os.killpg(os.getpgid(pid), signal.SIGTERM) - process.wait() - - rclpy.shutdown() - - -if __name__ == '__main__': - main() \ No newline at end of file diff --git a/capabilities2_launch_py/setup.cfg b/capabilities2_launch_py/setup.cfg deleted file mode 100644 index 5cc0c4a..0000000 --- a/capabilities2_launch_py/setup.cfg +++ /dev/null @@ -1,4 +0,0 @@ -[develop] -script_dir=$base/lib/capabilities2_launch_py -[install] -install_scripts=$base/lib/capabilities2_launch_py diff --git a/capabilities2_msgs/srv/Launch.srv b/capabilities2_msgs/srv/Launch.srv index b3eedd3..a1938ed 100644 --- a/capabilities2_msgs/srv/Launch.srv +++ b/capabilities2_msgs/srv/Launch.srv @@ -2,4 +2,5 @@ std_msgs/Header header string package_name string launch_file_name --- -std_msgs/Header header \ No newline at end of file +std_msgs/Header header +string status \ No newline at end of file diff --git a/capabilities2_runner/include/capabilities2_runner/launch_runner.hpp b/capabilities2_runner/include/capabilities2_runner/launch_runner.hpp index 1b74036..2697bf3 100644 --- a/capabilities2_runner/include/capabilities2_runner/launch_runner.hpp +++ b/capabilities2_runner/include/capabilities2_runner/launch_runner.hpp @@ -37,36 +37,36 @@ class LaunchRunner : public RunnerBase launch_name = run_config_.runner.substr(run_config_.runner.find("/") + 1); // create an service client - start_service_client_ = node_->create_client("/capabilities/launch_start"); + start_service_client_ = node_->create_client("/capabilities/launch/start"); - RCLCPP_INFO(node_->get_logger(), "%s waiting for service: /capabilities/launch_start", + RCLCPP_INFO(node_->get_logger(), "%s waiting for service: /capabilities/launch/start", run_config_.interface.c_str()); if (!start_service_client_->wait_for_service(std::chrono::seconds(3))) { - RCLCPP_ERROR(node_->get_logger(), "%s failed to connect to service: /capabilities/launch_start", + RCLCPP_ERROR(node_->get_logger(), "%s failed to connect to service: /capabilities/launch/start", run_config_.interface.c_str()); - throw runner_exception("Failed to connect to server: /capabilities/launch_start"); + throw runner_exception("Failed to connect to server: /capabilities/launch/start"); } - RCLCPP_INFO(node_->get_logger(), "%s connected to service: /capabilities/launch_start", + RCLCPP_INFO(node_->get_logger(), "%s connected to service: /capabilities/launch/start", run_config_.interface.c_str()); // create an service client - stop_service_client_ = node_->create_client("/capabilities/launch_stop"); + stop_service_client_ = node_->create_client("/capabilities/launch/stop"); // wait for action server - RCLCPP_INFO(node_->get_logger(), "%s waiting for service: /capabilities/launch_stop", + RCLCPP_INFO(node_->get_logger(), "%s waiting for service: /capabilities/launch/stop", run_config_.interface.c_str()); if (!stop_service_client_->wait_for_service(std::chrono::seconds(3))) { - RCLCPP_ERROR(node_->get_logger(), "%s failed to connect to service: /capabilities/launch_stop", + RCLCPP_ERROR(node_->get_logger(), "%s failed to connect to service: /capabilities/launch/stop", run_config_.interface.c_str()); - throw runner_exception("Failed to connect to server: /capabilities/launch_stop"); + throw runner_exception("Failed to connect to server: /capabilities/launch/stop"); } - RCLCPP_INFO(node_->get_logger(), "%s connected to service: /capabilities/launch_stop", + RCLCPP_INFO(node_->get_logger(), "%s connected to service: /capabilities/launch/stop", run_config_.interface.c_str()); // generate a reequest from launch_name and package_name diff --git a/capabilities2_runner_nav2/CMakeLists.txt b/capabilities2_runner_nav2/CMakeLists.txt index 3edafbc..1ce3c58 100644 --- a/capabilities2_runner_nav2/CMakeLists.txt +++ b/capabilities2_runner_nav2/CMakeLists.txt @@ -16,6 +16,7 @@ find_package(rclcpp_action REQUIRED) find_package(pluginlib REQUIRED) find_package(nav2_msgs REQUIRED) find_package(geometry_msgs REQUIRED) +find_package(lifecycle_msgs REQUIRED) find_package(capabilities2_msgs REQUIRED) find_package(capabilities2_runner REQUIRED) @@ -44,6 +45,7 @@ ament_target_dependencies(${PROJECT_NAME} pluginlib nav2_msgs geometry_msgs + lifecycle_msgs capabilities2_msgs capabilities2_runner ) 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 0a5cd6d..a28a2da 100644 --- a/capabilities2_runner_nav2/include/capabilities2_runner_nav2/waypoint_runner.hpp +++ b/capabilities2_runner_nav2/include/capabilities2_runner_nav2/waypoint_runner.hpp @@ -6,12 +6,15 @@ #include #include #include +#include #include #include #include +using namespace std::chrono_literals; + namespace capabilities2_runner { @@ -57,6 +60,9 @@ class WayPointRunner : public ActionRunner geometry_msgs::msg::PoseStamped pose_msg; pose_msg.header.frame_id = "map"; + pose_msg.header.stamp.sec = 0; + pose_msg.header.stamp.nanosec = 0; + pose_msg.pose.position.x = x; pose_msg.pose.position.y = y; pose_msg.pose.position.z = 0.0; @@ -87,6 +93,8 @@ class WayPointRunner : public ActionRunner std::string robot_base_frame_; /**The frame of the robot base*/ double x, y; /**Coordinate frame parameters*/ + + rclcpp::Client::SharedPtr get_state_client_; }; } // namespace capabilities2_runner From 375c30fb549cc981c8c2cc588316784616e05880 Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Thu, 16 Jan 2025 14:36:36 +1100 Subject: [PATCH 075/133] added lauunch proxy issue as a to-do taks --- TODO.md | 1 + 1 file changed, 1 insertion(+) diff --git a/TODO.md b/TODO.md index 0a7e405..171beac 100644 --- a/TODO.md +++ b/TODO.md @@ -5,6 +5,7 @@ - [x] names need to be in package/name format everywhere - [x] better docs - [ ] BUG: escape db function variables +- [ ] BUG: fix issue with connecting to services and actions started using launch proxy - [ ] close or change communication to launch proxy so that it can't be accessed from ros network - [ ] BUG: handle "'" in db queries From 4f759e467c908cc34a43587bf752004875fb5126 Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Sun, 26 Jan 2025 17:47:06 +1100 Subject: [PATCH 076/133] refactored code --- capabilities2_fabric/CMakeLists.txt | 10 ++-- capabilities2_fabric/config/fabric.yaml | 2 +- .../capabilities_fabric.hpp | 32 ++++++------ ...ties_file_parser.hpp => fabric_client.hpp} | 8 +-- .../capabilities2_fabric/utils/connection.hpp | 4 +- .../xml_parser.hpp} | 49 +++++++++---------- capabilities2_fabric/launch/fabric.launch.py | 4 +- ...ties_file_parser.cpp => fabric_client.cpp} | 4 +- 8 files changed, 55 insertions(+), 58 deletions(-) rename capabilities2_fabric/include/capabilities2_fabric/{capabilities_file_parser.hpp => fabric_client.hpp} (94%) rename capabilities2_fabric/include/capabilities2_fabric/{capabilities_xml_parser.hpp => utils/xml_parser.hpp} (70%) rename capabilities2_fabric/src/{capabilities_file_parser.cpp => fabric_client.cpp} (61%) diff --git a/capabilities2_fabric/CMakeLists.txt b/capabilities2_fabric/CMakeLists.txt index 9faebc0..c9879ea 100644 --- a/capabilities2_fabric/CMakeLists.txt +++ b/capabilities2_fabric/CMakeLists.txt @@ -48,21 +48,21 @@ install(TARGETS ${PROJECT_NAME} DESTINATION lib/${PROJECT_NAME} ) -add_executable(capabilities2_file_parser - src/capabilities_file_parser.cpp +add_executable(fabric_client + src/fabric_client.cpp ) -target_link_libraries(capabilities2_file_parser +target_link_libraries(fabric_client ${TINYXML2_LIB} ) -ament_target_dependencies(capabilities2_file_parser +ament_target_dependencies(fabric_client rclcpp rclcpp_action capabilities2_msgs ) -install(TARGETS capabilities2_file_parser +install(TARGETS fabric_client DESTINATION lib/${PROJECT_NAME} ) diff --git a/capabilities2_fabric/config/fabric.yaml b/capabilities2_fabric/config/fabric.yaml index f0f58b6..e12c542 100644 --- a/capabilities2_fabric/config/fabric.yaml +++ b/capabilities2_fabric/config/fabric.yaml @@ -1,4 +1,4 @@ -capabilities2_file_parser: +fabric_client: ros__parameters: plan_file_path: "install/capabilities2_fabric/share/capabilities2_fabric/plans/navigation_2.xml" # WaypointRunner Example 2 # plan_file_path: "install/capabilities2_fabric/share/capabilities2_fabric/plans/navigation_1.xml" # WaypointRunner Example 1 diff --git a/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric.hpp b/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric.hpp index 3deec48..d111c59 100644 --- a/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric.hpp +++ b/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric.hpp @@ -8,7 +8,7 @@ #include #include -#include +#include #include #include @@ -56,7 +56,7 @@ class CapabilitiesFabric : public rclcpp::Node CapabilitiesFabric() : Node("Capabilities2_Fabric") { - control_tag_list = capabilities2_xml_parser::get_control_list(); + control_tag_list = xml_parser::get_control_list(); } /** @@ -382,12 +382,12 @@ class CapabilitiesFabric : public rclcpp::Node print_and_feedback(goal_handle, "Plan verification successful", false); // extract the plan from the XMLDocument - tinyxml2::XMLElement* plan = capabilities2_xml_parser::get_plan(document); + tinyxml2::XMLElement* plan = xml_parser::get_plan(document); print_and_feedback(goal_handle, "Plan conversion successful", false); // Extract the connections from the plan - capabilities2_xml_parser::extract_connections(plan, connection_map); + xml_parser::extract_connections(plan, connection_map); print_and_feedback(goal_handle, "Connection extraction successful", false); @@ -410,7 +410,7 @@ class CapabilitiesFabric : public rclcpp::Node std::merge(interface_list.begin(), interface_list.end(), control_tag_list.begin(), control_tag_list.end(), tag_list.begin()); // verify whether document got 'plan' tags - if (!capabilities2_xml_parser::check_plan_tag(document)) + if (!xml_parser::check_plan_tag(document)) { print_and_result(goal_handle, "Execution plan is not compatible. Please recheck and update", false); return false; @@ -419,12 +419,12 @@ class CapabilitiesFabric : public rclcpp::Node print_and_feedback(goal_handle, "'Plan' tag checking successful", false); // extract the components within the 'plan' tags - tinyxml2::XMLElement* plan = capabilities2_xml_parser::get_plan(document); + tinyxml2::XMLElement* plan = xml_parser::get_plan(document); print_and_feedback(goal_handle, "Plan extraction complete", false); // verify whether the plan is valid - if (!capabilities2_xml_parser::check_tags(this->get_logger(), plan, interface_list, providers_list, control_tag_list, rejected_list)) + if (!xml_parser::check_tags(this->get_logger(), plan, interface_list, providers_list, control_tag_list, rejected_list)) { print_and_result(goal_handle, "Execution plan is faulty. Please recheck and update", false); return false; @@ -477,7 +477,7 @@ class CapabilitiesFabric : public rclcpp::Node * @param capabilities capability list to be started * @param provider provider of the capability */ - void use_capability(std::map& capabilities, const std::shared_ptr goal_handle) + void use_capability(std::map& capabilities, const std::shared_ptr goal_handle) { std::string capability = capabilities[completed_capabilities_].source.runner; std::string provider = capabilities[completed_capabilities_].source.provider; @@ -569,7 +569,7 @@ class CapabilitiesFabric : public rclcpp::Node /** * @brief Request use of capability from capabilities2 server */ - void configure_capabilities(std::map& capabilities, const std::shared_ptr goal_handle) + void configure_capabilities(std::map& capabilities, const std::shared_ptr goal_handle) { auto request_configure = std::make_shared(); @@ -577,7 +577,7 @@ class CapabilitiesFabric : public rclcpp::Node capabilities[completed_configurations_].source.runner; print_and_feedback(goal_handle, status, true); - if (capabilities2_xml_parser::convert_to_string(capabilities[completed_configurations_].source.parameters, request_configure->source.parameters)) + if (xml_parser::convert_to_string(capabilities[completed_configurations_].source.parameters, request_configure->source.parameters)) { request_configure->source.capability = capabilities[completed_configurations_].source.runner; request_configure->source.provider = capabilities[completed_configurations_].source.provider; @@ -591,7 +591,7 @@ class CapabilitiesFabric : public rclcpp::Node request_configure->source.provider = ""; } - if (capabilities2_xml_parser::convert_to_string(capabilities[completed_configurations_].target_on_start.parameters, + if (xml_parser::convert_to_string(capabilities[completed_configurations_].target_on_start.parameters, request_configure->target_on_start.parameters)) { request_configure->target_on_start.capability = capabilities[completed_configurations_].target_on_start.runner; @@ -606,7 +606,7 @@ class CapabilitiesFabric : public rclcpp::Node request_configure->target_on_start.provider = ""; } - if (capabilities2_xml_parser::convert_to_string(capabilities[completed_configurations_].target_on_stop.parameters, + if (xml_parser::convert_to_string(capabilities[completed_configurations_].target_on_stop.parameters, request_configure->target_on_stop.parameters)) { request_configure->target_on_stop.capability = capabilities[completed_configurations_].target_on_stop.runner; @@ -621,7 +621,7 @@ class CapabilitiesFabric : public rclcpp::Node request_configure->target_on_stop.provider = ""; } - if (capabilities2_xml_parser::convert_to_string(capabilities[completed_configurations_].target_on_success.parameters, + if (xml_parser::convert_to_string(capabilities[completed_configurations_].target_on_success.parameters, request_configure->target_on_success.parameters)) { request_configure->target_on_success.capability = capabilities[completed_configurations_].target_on_success.runner; @@ -636,7 +636,7 @@ class CapabilitiesFabric : public rclcpp::Node request_configure->target_on_success.provider = ""; } - if (capabilities2_xml_parser::convert_to_string(capabilities[completed_configurations_].target_on_failure.parameters, + if (xml_parser::convert_to_string(capabilities[completed_configurations_].target_on_failure.parameters, request_configure->target_on_failure.parameters)) { request_configure->target_on_failure.capability = capabilities[completed_configurations_].target_on_failure.runner; @@ -693,7 +693,7 @@ class CapabilitiesFabric : public rclcpp::Node auto request_trigger = std::make_shared(); std::string parameter_string; - capabilities2_xml_parser::convert_to_string(connection_map[0].source.parameters, parameter_string); + xml_parser::convert_to_string(connection_map[0].source.parameters, parameter_string); request_trigger->capability = connection_map[0].source.runner; request_trigger->parameters = parameter_string; @@ -790,7 +790,7 @@ class CapabilitiesFabric : public rclcpp::Node tinyxml2::XMLDocument document; /** vector of connections */ - std::map connection_map; + std::map connection_map; /** Interface List */ std::vector is_semantic_list; diff --git a/capabilities2_fabric/include/capabilities2_fabric/capabilities_file_parser.hpp b/capabilities2_fabric/include/capabilities2_fabric/fabric_client.hpp similarity index 94% rename from capabilities2_fabric/include/capabilities2_fabric/capabilities_file_parser.hpp rename to capabilities2_fabric/include/capabilities2_fabric/fabric_client.hpp index d7614e9..a4d9a49 100644 --- a/capabilities2_fabric/include/capabilities2_fabric/capabilities_file_parser.hpp +++ b/capabilities2_fabric/include/capabilities2_fabric/fabric_client.hpp @@ -10,7 +10,7 @@ #include #include -#include +#include #include /** @@ -20,10 +20,10 @@ * Will read an XML file that implements a plan and send it to the server */ -class CapabilitiesFileParser : public rclcpp::Node +class CapabilitiesFabricClient : public rclcpp::Node { public: - CapabilitiesFileParser() : Node("Capabilities2_File_Parser") + CapabilitiesFabricClient() : Node("Capabilities2_Fabric_Client") { declare_parameter("plan_file_path", "install/capabilities2_fabric/share/capabilities2_fabric/plans/default.xml"); plan_file_path = get_parameter("plan_file_path").as_string(); @@ -66,7 +66,7 @@ class CapabilitiesFileParser : public rclcpp::Node auto goal_msg = capabilities2_msgs::action::Plan::Goal(); - capabilities2_xml_parser::convert_to_string(document, goal_msg.plan); + xml_parser::convert_to_string(document, goal_msg.plan); RCLCPP_INFO(this->get_logger(), "Following plan was loaded :\n\n%s ", goal_msg.plan.c_str()); diff --git a/capabilities2_fabric/include/capabilities2_fabric/utils/connection.hpp b/capabilities2_fabric/include/capabilities2_fabric/utils/connection.hpp index d6ecc65..4840bc8 100644 --- a/capabilities2_fabric/include/capabilities2_fabric/utils/connection.hpp +++ b/capabilities2_fabric/include/capabilities2_fabric/utils/connection.hpp @@ -1,7 +1,7 @@ #include #include -namespace capabilities2_executor +namespace capabilities2 { enum connection_type_t { @@ -26,4 +26,4 @@ namespace capabilities2_executor connection_t target_on_failure; }; -} // namespace capabilities2_executor +} // namespace capabilities2 diff --git a/capabilities2_fabric/include/capabilities2_fabric/capabilities_xml_parser.hpp b/capabilities2_fabric/include/capabilities2_fabric/utils/xml_parser.hpp similarity index 70% rename from capabilities2_fabric/include/capabilities2_fabric/capabilities_xml_parser.hpp rename to capabilities2_fabric/include/capabilities2_fabric/utils/xml_parser.hpp index 6910d60..c9085dd 100644 --- a/capabilities2_fabric/include/capabilities2_fabric/capabilities_xml_parser.hpp +++ b/capabilities2_fabric/include/capabilities2_fabric/utils/xml_parser.hpp @@ -4,7 +4,7 @@ #include #include -namespace capabilities2_xml_parser +namespace xml_parser { /** * @brief extract elements related plan @@ -138,10 +138,10 @@ bool check_tags(rclcpp::Logger logger, tinyxml2::XMLElement* element, std::vecto providertag = ""; bool hasChildren = !element->NoChildren(); - bool hasSiblings = !capabilities2_xml_parser::isLastElement(element); - bool foundInControl = capabilities2_xml_parser::search(control, nametag); - bool foundInEvents = capabilities2_xml_parser::search(events, nametag); - bool foundInProviders = capabilities2_xml_parser::search(providers, providertag); + bool hasSiblings = !xml_parser::isLastElement(element); + bool foundInControl = xml_parser::search(control, nametag); + bool foundInEvents = xml_parser::search(events, nametag); + bool foundInProviders = xml_parser::search(providers, providertag); bool returnValue = true; if (typetag == "Control") @@ -154,10 +154,10 @@ bool check_tags(rclcpp::Logger logger, tinyxml2::XMLElement* element, std::vecto } if (hasChildren) - returnValue &= capabilities2_xml_parser::check_tags(logger, element->FirstChildElement(), events, providers, control, rejected); + returnValue &= xml_parser::check_tags(logger, element->FirstChildElement(), events, providers, control, rejected); if (hasSiblings) - returnValue &= capabilities2_xml_parser::check_tags(logger, element->NextSiblingElement(), events, providers, control, rejected); + returnValue &= xml_parser::check_tags(logger, element->NextSiblingElement(), events, providers, control, rejected); } else if (typetag == "Event") { @@ -169,7 +169,7 @@ bool check_tags(rclcpp::Logger logger, tinyxml2::XMLElement* element, std::vecto } if (hasSiblings) - returnValue &= capabilities2_xml_parser::check_tags(logger, element->NextSiblingElement(), events, providers, control, rejected); + returnValue &= xml_parser::check_tags(logger, element->NextSiblingElement(), events, providers, control, rejected); } else { @@ -204,8 +204,8 @@ std::vector get_control_list() * @param connection_id numerical id of the connection * @param connection_type the type of connection */ -void extract_connections(tinyxml2::XMLElement* element, std::map& connections, int connection_id = 0, - capabilities2_executor::connection_type_t connection_type = capabilities2_executor::connection_type_t::ON_SUCCESS) +void extract_connections(tinyxml2::XMLElement* element, std::map& connections, int connection_id = 0, + capabilities2::connection_type_t connection_type = capabilities2::connection_type_t::ON_SUCCESS) { int predecessor_id; @@ -231,38 +231,35 @@ void extract_connections(tinyxml2::XMLElement* element, std::mapNoChildren(); - bool hasSiblings = !capabilities2_xml_parser::isLastElement(element); + bool hasSiblings = !xml_parser::isLastElement(element); if ((typetag == "Control") and (nametag == "sequential")) { if (hasChildren) - capabilities2_xml_parser::extract_connections(element->FirstChildElement(), connections, connection_id, - capabilities2_executor::connection_type_t::ON_SUCCESS); + xml_parser::extract_connections(element->FirstChildElement(), connections, connection_id, capabilities2::connection_type_t::ON_SUCCESS); if (hasSiblings) - capabilities2_xml_parser::extract_connections(element->NextSiblingElement(), connections, connection_id, connection_type); + xml_parser::extract_connections(element->NextSiblingElement(), connections, connection_id, connection_type); } else if ((typetag == "Control") and (nametag == "parallel")) { if (hasChildren) - capabilities2_xml_parser::extract_connections(element->FirstChildElement(), connections, connection_id, - capabilities2_executor::connection_type_t::ON_START); + xml_parser::extract_connections(element->FirstChildElement(), connections, connection_id, capabilities2::connection_type_t::ON_START); if (hasSiblings) - capabilities2_xml_parser::extract_connections(element->NextSiblingElement(), connections, connection_id, connection_type); + xml_parser::extract_connections(element->NextSiblingElement(), connections, connection_id, connection_type); } else if ((typetag == "Control") and (nametag == "recovery")) { if (hasChildren) - capabilities2_xml_parser::extract_connections(element->FirstChildElement(), connections, connection_id, - capabilities2_executor::connection_type_t::ON_FAILURE); + xml_parser::extract_connections(element->FirstChildElement(), connections, connection_id, capabilities2::connection_type_t::ON_FAILURE); if (hasSiblings) - capabilities2_xml_parser::extract_connections(element->NextSiblingElement(), connections, connection_id, connection_type); + xml_parser::extract_connections(element->NextSiblingElement(), connections, connection_id, connection_type); } else if (typetag == "Event") { - capabilities2_executor::node_t node; + capabilities2::node_t node; node.source.runner = nametag; node.source.provider = providertag; @@ -275,18 +272,18 @@ void extract_connections(tinyxml2::XMLElement* element, std::mapNextSiblingElement(), connections, connection_id + 1, connection_type); + xml_parser::extract_connections(element->NextSiblingElement(), connections, connection_id + 1, connection_type); } } -} // namespace capabilities2_xml_parser +} // namespace xml_parser diff --git a/capabilities2_fabric/launch/fabric.launch.py b/capabilities2_fabric/launch/fabric.launch.py index 2320523..d0e6dfc 100644 --- a/capabilities2_fabric/launch/fabric.launch.py +++ b/capabilities2_fabric/launch/fabric.launch.py @@ -28,8 +28,8 @@ def generate_launch_description(): executor_file = Node( package='capabilities2_fabric', namespace='', - executable='capabilities2_file_parser', - name='capabilities2_file_parser', + executable='fabric_client', + name='fabric_client', parameters=[executor_config], output='screen' ) diff --git a/capabilities2_fabric/src/capabilities_file_parser.cpp b/capabilities2_fabric/src/fabric_client.cpp similarity index 61% rename from capabilities2_fabric/src/capabilities_file_parser.cpp rename to capabilities2_fabric/src/fabric_client.cpp index bb08cf0..411a2f0 100644 --- a/capabilities2_fabric/src/capabilities_file_parser.cpp +++ b/capabilities2_fabric/src/fabric_client.cpp @@ -1,10 +1,10 @@ -#include +#include int main(int argc, char* argv[]) { rclcpp::init(argc, argv); - auto parser_node = std::make_shared(); + auto parser_node = std::make_shared(); parser_node->initialize(); // Call initialize after construction From 588051f1798569a6e1d593aa8cf2d699d932d97c Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Tue, 28 Jan 2025 00:02:15 +1100 Subject: [PATCH 077/133] refactored code and added status publishing --- TODO.md | 11 +- capabilities2_fabric/CMakeLists.txt | 3 + .../capabilities_fabric.hpp | 319 ++++++++++-------- .../capabilities2_fabric/fabric_client.hpp | 2 +- .../capabilities2_fabric/utils/xml_parser.hpp | 289 ---------------- capabilities2_fabric/launch/fabric.launch.py | 13 +- capabilities2_fabric/package.xml | 1 + capabilities2_msgs/CMakeLists.txt | 3 + capabilities2_msgs/msg/Status.msg | 7 + capabilities2_msgs/srv/GetFabricStatus.srv | 7 + capabilities2_msgs/srv/SetFabricPlan.srv | 5 + capabilities2_server/CMakeLists.txt | 3 + .../capabilities2_server/capabilities_db.hpp | 3 +- .../capabilities2_server/models/header.hpp | 2 +- .../capabilities2_server/models/interface.hpp | 2 +- .../capabilities2_server/models/provider.hpp | 2 +- .../models/semantic_interface.hpp | 2 +- capabilities2_server/package.xml | 1 + capabilities2_utils/CMakeLists.txt | 24 ++ .../capabilities2_utils}/bond_client.hpp | 1 + .../capabilities2_utils}/connection.hpp | 1 + .../include/capabilities2_utils}/sql_safe.hpp | 0 .../capabilities2_utils/status_client.hpp | 110 ++++++ .../capabilities2_utils/xml_parser.hpp | 295 ++++++++++++++++ capabilities2_utils/package.xml | 21 ++ 25 files changed, 673 insertions(+), 454 deletions(-) delete mode 100644 capabilities2_fabric/include/capabilities2_fabric/utils/xml_parser.hpp create mode 100644 capabilities2_msgs/msg/Status.msg create mode 100644 capabilities2_msgs/srv/GetFabricStatus.srv create mode 100644 capabilities2_msgs/srv/SetFabricPlan.srv create mode 100644 capabilities2_utils/CMakeLists.txt rename {capabilities2_fabric/include/capabilities2_fabric/utils => capabilities2_utils/include/capabilities2_utils}/bond_client.hpp (99%) rename {capabilities2_fabric/include/capabilities2_fabric/utils => capabilities2_utils/include/capabilities2_utils}/connection.hpp (97%) rename {capabilities2_server/include/capabilities2_server/sanitizer => capabilities2_utils/include/capabilities2_utils}/sql_safe.hpp (100%) create mode 100644 capabilities2_utils/include/capabilities2_utils/status_client.hpp create mode 100644 capabilities2_utils/include/capabilities2_utils/xml_parser.hpp create mode 100644 capabilities2_utils/package.xml diff --git a/TODO.md b/TODO.md index 171beac..0808968 100644 --- a/TODO.md +++ b/TODO.md @@ -4,15 +4,16 @@ - [x] names need to be in package/name format everywhere - [x] better docs -- [ ] BUG: escape db function variables -- [ ] BUG: fix issue with connecting to services and actions started using launch proxy +- [x] BUG: handle "'" in db queries +- [x] BUG: escape db function variables - [ ] close or change communication to launch proxy so that it can't be accessed from ros network -- [ ] BUG: handle "'" in db queries +- [ ] BUG: fix issue with connecting to services and actions started using launch proxy + ## Features +- [x] try using ros package to find exports automatically +- [x] improve the event system - [ ] implement provider definition handling in runner -- [ ] try using ros package to find exports automatically -- [ ] improve the event system - [ ] move to established db handler lib - [ ] better bt runner impl diff --git a/capabilities2_fabric/CMakeLists.txt b/capabilities2_fabric/CMakeLists.txt index c9879ea..3cd4cfb 100644 --- a/capabilities2_fabric/CMakeLists.txt +++ b/capabilities2_fabric/CMakeLists.txt @@ -17,6 +17,7 @@ find_package(rclcpp_action REQUIRED) find_package(capabilities2_msgs REQUIRED) find_package(bondcpp REQUIRED) find_package(backward_ros REQUIRED) +find_package(capabilities2_utils REQUIRED) # Locate the static version of tinyxml2 find_library(TINYXML2_LIB NAMES tinyxml2 PATHS /usr/local/lib NO_DEFAULT_PATH) @@ -42,6 +43,7 @@ ament_target_dependencies(${PROJECT_NAME} rclcpp_action bondcpp capabilities2_msgs + capabilities2_utils ) install(TARGETS ${PROJECT_NAME} @@ -60,6 +62,7 @@ ament_target_dependencies(fabric_client rclcpp rclcpp_action capabilities2_msgs + capabilities2_utils ) install(TARGETS fabric_client diff --git a/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric.hpp b/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric.hpp index d111c59..bc16c7d 100644 --- a/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric.hpp +++ b/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric.hpp @@ -8,8 +8,9 @@ #include #include -#include -#include +#include +#include +#include #include @@ -60,11 +61,15 @@ class CapabilitiesFabric : public rclcpp::Node } /** - * @brief Initializer function + * @brief Initializer function for Ccapabilities2 fabric. + * Configure the action server for the capabilieites fabric and configure server clients for the capability runners from the + * capabilities2 server * */ void initialize() { + status_ = std::make_shared(shared_from_this(), "capabilities_fabric", "/status/capabilities_fabric"); + this->planner_server_ = rclcpp_action::create_server( this, "/capabilities_fabric", std::bind(&CapabilitiesFabric::handle_goal, this, std::placeholders::_1, std::placeholders::_2), std::bind(&CapabilitiesFabric::handle_cancel, this, std::placeholders::_1), @@ -104,7 +109,8 @@ class CapabilitiesFabric : public rclcpp::Node */ rclcpp_action::GoalResponse handle_goal(const rclcpp_action::GoalUUID& uuid, std::shared_ptr goal) { - RCLCPP_INFO(this->get_logger(), "Received the goal request with the plan"); + status_->info("Received the goal request with the plan"); + (void)uuid; // try to parse the std::string plan from capabilities_msgs/Plan to the to a XMLDocument file @@ -113,11 +119,11 @@ class CapabilitiesFabric : public rclcpp::Node // check if the file parsing failed if (xml_status != tinyxml2::XMLError::XML_SUCCESS) { - RCLCPP_ERROR(this->get_logger(), "Parsing the plan from goal message failed"); + status_->error("Parsing the plan from goal message failed"); return rclcpp_action::GoalResponse::REJECT; } - RCLCPP_INFO(this->get_logger(), "Plan parsed and accepted"); + status_->info("Plan parsed and accepted"); return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; } @@ -130,9 +136,23 @@ class CapabilitiesFabric : public rclcpp::Node */ rclcpp_action::CancelResponse handle_cancel(const std::shared_ptr goal_handle) { - RCLCPP_ERROR(this->get_logger(), "Received the request to cancel the plan"); + status_->error("Received the request to cancel the plan"); (void)goal_handle; + if (connection_map.size() > 0) + { + // release all capabilities that were used since cancel request came + for (const auto& [key, value] : connection_map) + { + status = "Freeing capability of Node " + std::to_string(key) + " named " + value.source.runner; + process_feedback(status); + + free_capability(value.source.runner); + } + + bond_client_->stop(); + } + return rclcpp_action::CancelResponse::ACCEPT; } @@ -143,15 +163,17 @@ class CapabilitiesFabric : public rclcpp::Node */ void handle_accepted(const std::shared_ptr goal_handle) { - execution(goal_handle); + goal_handle_ = goal_handle; + + execution(); } /** - * @brief Trigger execution + * @brief Start the execution of the capabilities2 fabric */ - void execution(const std::shared_ptr goal_handle) + void execution() { - print_and_feedback(goal_handle, "Execution started", false); + process_feedback("Execution started"); expected_providers_ = 0; completed_providers_ = 0; @@ -165,25 +187,25 @@ class CapabilitiesFabric : public rclcpp::Node expected_configurations_ = 0; completed_configurations_ = 0; - getInterfaces(goal_handle); + getInterfaces(); } /** - * @brief Get Interfaces + * @brief Get Interfaces available in the capabilities2 server via relavant service */ - void getInterfaces(const std::shared_ptr goal_handle) + void getInterfaces() { - print_and_feedback(goal_handle, "Requesting Interface information", false); + process_feedback("Requesting Interface information"); auto request_interface = std::make_shared(); // request data from the server - auto result_future = get_interfaces_client_->async_send_request(request_interface, [this, goal_handle](GetInterfacesClient::SharedFuture future) { + auto result_future = get_interfaces_client_->async_send_request(request_interface, [this](GetInterfacesClient::SharedFuture future) { auto result = std::make_shared(); if (!future.valid()) { - print_and_result(goal_handle, "Failed to get Interface information. Server execution cancelled", false); + process_result("Failed to get Interface information. Server execution cancelled", false, false); return; } @@ -191,32 +213,34 @@ class CapabilitiesFabric : public rclcpp::Node expected_interfaces_ = response->interfaces.size(); status = "Received Interfaces. Requsting " + std::to_string(expected_interfaces_) + " semantic interface information"; - print_and_feedback(goal_handle, status, false); + process_feedback(status); // Request each interface recursively for Semantic interfaces - getSemanticInterfaces(response->interfaces, goal_handle); + getSemanticInterfaces(response->interfaces); }); } /** - * @brief Get Semantic Interfaces + * @brief Get the Semantic Interfaces from the capabilities2 server via related service client + * + * @param interfaces std::vector of interfaces for which the semantic interfaces will be requested */ - void getSemanticInterfaces(const std::vector& interfaces, const std::shared_ptr goal_handle) + void getSemanticInterfaces(const std::vector& interfaces) { std::string requested_interface = interfaces[completed_interfaces_]; status = "Requesting semantic interfaces for " + requested_interface; - print_and_feedback(goal_handle, status, true); + process_feedback(status, true); auto request_semantic = std::make_shared(); request_semantic->interface = requested_interface; // request semantic interface from the server auto result_semantic_future = get_sem_interf_client_->async_send_request( - request_semantic, [this, goal_handle, interfaces, requested_interface](GetSemanticInterfacesClient::SharedFuture future) { + request_semantic, [this, interfaces, requested_interface](GetSemanticInterfacesClient::SharedFuture future) { if (!future.valid()) { - print_and_result(goal_handle, "Failed to get Semantic Interface information. Server execution cancelled", false); + process_result("Failed to get Semantic Interface information. Server execution cancelled", false, false); return; } @@ -233,7 +257,7 @@ class CapabilitiesFabric : public rclcpp::Node status = (completed_interfaces_) + "/" + std::to_string(expected_interfaces_) + " : Received " + semantic_interface + " for " + requested_interface + ". So added " + semantic_interface; - print_and_feedback(goal_handle, status, false); + process_feedback(status); } } // if no semantic interfaces are availble for a given interface, add the interface instead @@ -244,41 +268,42 @@ class CapabilitiesFabric : public rclcpp::Node status = std::to_string(completed_interfaces_) + "/" + std::to_string(expected_interfaces_) + " : Received none for " + requested_interface + ". So added " + requested_interface; - print_and_feedback(goal_handle, status, false); + process_feedback(status); } if (completed_interfaces_ != expected_interfaces_) { // Request next interface recursively for Semantic interfaces - getSemanticInterfaces(interfaces, goal_handle); + getSemanticInterfaces(interfaces); } else { - print_and_feedback(goal_handle, "Received all requested Interface information", true); + process_feedback("Received all requested Interface information", true); expected_providers_ = interface_list.size(); status = "Requsting Provider information for " + std::to_string(expected_providers_) + " providers"; - print_and_feedback(goal_handle, status, false); + process_feedback(status); // request providers from the interfaces in the interfaces_list - getProvider(interface_list, is_semantic_list, goal_handle); + getProvider(interface_list, is_semantic_list); } }); } /** - * @brief Get Providers + * @brief Get the Provider information for the related interfaces * + * @param interfaces std::vector of interfaces + * @param is_semantic std::vector of masks about interfaces with true value for semantic interfaces */ - void getProvider(const std::vector& interfaces, const std::vector& is_semantic, - const std::shared_ptr goal_handle) + void getProvider(const std::vector& interfaces, const std::vector& is_semantic) { std::string requested_interface = interfaces[completed_providers_]; bool semantic_flag = is_semantic[completed_providers_]; status = "Requesting provider for " + requested_interface; - print_and_feedback(goal_handle, status, true); + process_feedback(status, true); auto request_providers = std::make_shared(); @@ -287,11 +312,11 @@ class CapabilitiesFabric : public rclcpp::Node request_providers->include_semantic = semantic_flag; auto result_providers_future = get_providers_client_->async_send_request( - request_providers, [this, is_semantic, requested_interface, interfaces, goal_handle](GetProvidersClient::SharedFuture future) { + request_providers, [this, is_semantic, requested_interface, interfaces](GetProvidersClient::SharedFuture future) { if (!future.valid()) { status = "Did not retrieve providers for interface: " + requested_interface; - print_and_result(goal_handle, status, false); + process_result(status, false, false); return; } @@ -305,7 +330,7 @@ class CapabilitiesFabric : public rclcpp::Node status = std::to_string(completed_providers_) + "/" + std::to_string(expected_providers_) + " : Received " + response->default_provider + " for " + requested_interface + ". So added " + response->default_provider; - print_and_feedback(goal_handle, status, false); + process_feedback(status); } // add additional providers to the list if available @@ -317,42 +342,43 @@ class CapabilitiesFabric : public rclcpp::Node status = std::to_string(completed_providers_) + "/" + std::to_string(expected_providers_) + " : Received and added " + provider + " for " + requested_interface; - print_and_feedback(goal_handle, status, false); + process_feedback(status); } } else { status = std::to_string(completed_providers_) + "/" + std::to_string(expected_providers_) + " : No providers for " + requested_interface; - print_and_feedback(goal_handle, status, false); + process_feedback(status); } // Check if all expected calls are completed before calling verify_plan if (completed_providers_ != expected_providers_) { // request providers for the next interface in the interfaces_list - getProvider(interfaces, is_semantic, goal_handle); + getProvider(interfaces, is_semantic); } else { - print_and_feedback(goal_handle, "All requested interface, semantic interface and provider data recieved", true); + process_feedback("All requested interface, semantic interface and provider data recieved", true); - verify_and_continue(goal_handle); + verify_and_continue(); } }); } /** - * @brief Verify the plan and continue the execution + * @brief Verify the plan before continuing the execution using xml parsing and collected interface, semantic interface + * and provider information * */ - void verify_and_continue(const std::shared_ptr goal_handle) + void verify_and_continue() { - print_and_feedback(goal_handle, "Verifying the plan", false); + process_feedback("Verifying the plan"); // verify the plan - if (!verify_plan(goal_handle)) + if (!verify_plan()) { - print_and_feedback(goal_handle, "Plan verification failed", false); + process_feedback("Plan verification failed"); if (rejected_list.size() > 0) { @@ -361,38 +387,40 @@ class CapabilitiesFabric : public rclcpp::Node result->success = false; result->message = "Plan verification failed. There are mismatched events"; + process_feedback(result->message); + for (const auto& rejected_element : rejected_list) { - RCLCPP_ERROR(this->get_logger(), "Failed Events : %s", rejected_element.c_str()); + status_->error_element(rejected_element); result->failed_elements.push_back(rejected_element); } - goal_handle->abort(result); - RCLCPP_ERROR(this->get_logger(), "Server Execution Cancelled"); + goal_handle_->abort(result); + status_->error("Server Execution Cancelled"); } else { // TODO: improve with error codes - print_and_result(goal_handle, "Plan verification failed. Server Execution Cancelled.", false); + process_result("Plan verification failed. Server Execution Cancelled."); } - RCLCPP_ERROR(this->get_logger(), "Server Execution Cancelled"); + status_->error("Server Execution Cancelled"); } - print_and_feedback(goal_handle, "Plan verification successful", false); + process_feedback("Plan verification successful"); // extract the plan from the XMLDocument tinyxml2::XMLElement* plan = xml_parser::get_plan(document); - print_and_feedback(goal_handle, "Plan conversion successful", false); + process_feedback("Plan conversion successful"); // Extract the connections from the plan xml_parser::extract_connections(plan, connection_map); - print_and_feedback(goal_handle, "Connection extraction successful", false); + process_feedback("Connection extraction successful"); // estasblish the bond with the server - establish_bond(goal_handle); + establish_bond(); } /** @@ -400,7 +428,7 @@ class CapabilitiesFabric : public rclcpp::Node * * @return `true` if interface retreival is successful,`false` otherwise */ - bool verify_plan(const std::shared_ptr goal_handle) + bool verify_plan() { auto feedback = std::make_shared(); auto result = std::make_shared(); @@ -412,25 +440,25 @@ class CapabilitiesFabric : public rclcpp::Node // verify whether document got 'plan' tags if (!xml_parser::check_plan_tag(document)) { - print_and_result(goal_handle, "Execution plan is not compatible. Please recheck and update", false); + process_result("Execution plan is not compatible. Please recheck and update"); return false; } - print_and_feedback(goal_handle, "'Plan' tag checking successful", false); + process_feedback("'Plan' tag checking successful"); // extract the components within the 'plan' tags tinyxml2::XMLElement* plan = xml_parser::get_plan(document); - print_and_feedback(goal_handle, "Plan extraction complete", false); + process_feedback("Plan extraction complete"); // verify whether the plan is valid - if (!xml_parser::check_tags(this->get_logger(), plan, interface_list, providers_list, control_tag_list, rejected_list)) + if (!xml_parser::check_tags(status_, plan, interface_list, providers_list, control_tag_list, rejected_list)) { - print_and_result(goal_handle, "Execution plan is faulty. Please recheck and update", false); + process_result("Execution plan is faulty. Please recheck and update"); return false; } - print_and_feedback(goal_handle, "Checking tags successful", false); + process_feedback("Checking tags successful"); return true; } @@ -438,18 +466,18 @@ class CapabilitiesFabric : public rclcpp::Node * @brief establish the bond with capabilities2 server * */ - void establish_bond(const std::shared_ptr goal_handle) + void establish_bond() { - print_and_feedback(goal_handle, "Requesting bond id", false); + process_feedback("Requesting bond id"); // create bond establishing server request auto request_bond = std::make_shared(); // send the request - auto result_future = establish_bond_client_->async_send_request(request_bond, [this, goal_handle](EstablishBondClient::SharedFuture future) { + auto result_future = establish_bond_client_->async_send_request(request_bond, [this](EstablishBondClient::SharedFuture future) { if (!future.valid()) { - print_and_result(goal_handle, "Failed to retrieve the bond id. Server execution cancelled", false); + process_result("Failed to retrieve the bond id. Server execution cancelled"); return; } @@ -457,17 +485,17 @@ class CapabilitiesFabric : public rclcpp::Node bond_id_ = response->bond_id; status = "Received the bond id : " + bond_id_; - print_and_feedback(goal_handle, status, false); + process_feedback(status); bond_client_ = std::make_unique(shared_from_this(), bond_id_); bond_client_->start(); expected_capabilities_ = connection_map.size(); - status = "Requsting start of " + std::to_string(expected_capabilities_) + "capabilities"; - print_and_feedback(goal_handle, status, false); + status = "Requsting start of " + std::to_string(expected_capabilities_) + " capabilities"; + process_feedback(status); - use_capability(connection_map, goal_handle); + use_capability(connection_map); }); } @@ -477,7 +505,7 @@ class CapabilitiesFabric : public rclcpp::Node * @param capabilities capability list to be started * @param provider provider of the capability */ - void use_capability(std::map& capabilities, const std::shared_ptr goal_handle) + void use_capability(std::map& capabilities) { std::string capability = capabilities[completed_capabilities_].source.runner; std::string provider = capabilities[completed_capabilities_].source.provider; @@ -487,26 +515,28 @@ class CapabilitiesFabric : public rclcpp::Node request_use->preferred_provider = provider; request_use->bond_id = bond_id_; - status = "Starting capability of Node " + std::to_string(completed_capabilities_) + " : " + capabilities[completed_capabilities_].source.runner; - print_and_feedback(goal_handle, status, true); + status = "Starting capability of Runner " + std::to_string(completed_capabilities_) + " : " + capabilities[completed_capabilities_].source.runner; + process_feedback(status, true); // send the request auto result_future = - use_capability_client_->async_send_request(request_use, [this, goal_handle, capability, provider](UseCapabilityClient::SharedFuture future) { + use_capability_client_->async_send_request(request_use, [this, capability, provider](UseCapabilityClient::SharedFuture future) { if (!future.valid()) { status = "Failed to Use capability " + capability + " from " + provider + ". Server Execution Cancelled"; - print_and_result(goal_handle, status, false); + process_result(status); // release all capabilities that were used since not all started successfully for (const auto& [key, value] : connection_map) { status = "Freeing capability of Node " + std::to_string(key) + " named " + value.source.runner; - print_and_feedback(goal_handle, status, false); + process_feedback(status); - free_capability(value.source.runner, goal_handle); + free_capability(value.source.runner); } + bond_client_->stop(); + return; } @@ -515,23 +545,23 @@ class CapabilitiesFabric : public rclcpp::Node auto response = future.get(); status = std::to_string(completed_capabilities_) + "/" + std::to_string(expected_capabilities_) + " : start succeessful"; - print_and_feedback(goal_handle, status, true); + process_feedback(status); // Check if all expected calls are completed before calling verify_plan if (completed_capabilities_ == expected_capabilities_) { - print_and_feedback(goal_handle, "All requested capabilities have been started. Configuring the capabilities with events", true); + process_feedback("All requested capabilities have been started. Configuring the capabilities with events", true); expected_configurations_ = connection_map.size(); - status = "Requsting capability configuration for " + std::to_string(expected_configurations_) + "capabilities"; - print_and_feedback(goal_handle, status, true); + status = "Requsting capability configuration for " + std::to_string(expected_configurations_) + " capabilities"; + process_feedback(status, true); - configure_capabilities(connection_map, goal_handle); + configure_capabilities(connection_map); } else { - use_capability(connection_map, goal_handle); + use_capability(connection_map); } }); } @@ -541,49 +571,43 @@ class CapabilitiesFabric : public rclcpp::Node * * @param capability capability name to be started */ - void free_capability(const std::string& capability, const std::shared_ptr goal_handle) + void free_capability(const std::string& capability) { auto request_free = std::make_shared(); request_free->capability = capability; request_free->bond_id = bond_id_; // send the request - auto result_future = - free_capability_client_->async_send_request(request_free, [this, goal_handle, capability](FreeCapabilityClient::SharedFuture future) { - if (!future.valid()) - { - status = "Failed to free capability " + capability; - print_and_result(goal_handle, status, false); - return; - } - - auto response = future.get(); + auto result_future = free_capability_client_->async_send_request(request_free, [this, capability](FreeCapabilityClient::SharedFuture future) { + if (!future.valid()) + { + status = "Failed to free capability " + capability; + process_result(status); + return; + } - status = "Successfully freed capability " + capability; - print_and_feedback(goal_handle, status, true); + auto response = future.get(); - bond_client_->stop(); - }); + status = "Successfully freed capability " + capability; + process_feedback(status, true); + }); } /** * @brief Request use of capability from capabilities2 server */ - void configure_capabilities(std::map& capabilities, const std::shared_ptr goal_handle) + void configure_capabilities(std::map& capabilities) { auto request_configure = std::make_shared(); - status = "Configuring capability of Node " + std::to_string(completed_configurations_) + " named " + + status = "Configuring capability of Runner " + std::to_string(completed_configurations_) + " named " + capabilities[completed_configurations_].source.runner; - print_and_feedback(goal_handle, status, true); + process_feedback(status, true); if (xml_parser::convert_to_string(capabilities[completed_configurations_].source.parameters, request_configure->source.parameters)) { request_configure->source.capability = capabilities[completed_configurations_].source.runner; request_configure->source.provider = capabilities[completed_configurations_].source.provider; - - RCLCPP_INFO(this->get_logger(), "Source capability : %s from provider : %s", request_configure->source.capability.c_str(), - request_configure->source.provider.c_str()); } else { @@ -592,13 +616,10 @@ class CapabilitiesFabric : public rclcpp::Node } if (xml_parser::convert_to_string(capabilities[completed_configurations_].target_on_start.parameters, - request_configure->target_on_start.parameters)) + request_configure->target_on_start.parameters)) { request_configure->target_on_start.capability = capabilities[completed_configurations_].target_on_start.runner; request_configure->target_on_start.provider = capabilities[completed_configurations_].target_on_start.provider; - - RCLCPP_INFO(this->get_logger(), "--> on_start capability : %s from provider : %s", request_configure->target_on_start.capability.c_str(), - request_configure->target_on_start.provider.c_str()); } else { @@ -607,13 +628,10 @@ class CapabilitiesFabric : public rclcpp::Node } if (xml_parser::convert_to_string(capabilities[completed_configurations_].target_on_stop.parameters, - request_configure->target_on_stop.parameters)) + request_configure->target_on_stop.parameters)) { request_configure->target_on_stop.capability = capabilities[completed_configurations_].target_on_stop.runner; request_configure->target_on_stop.provider = capabilities[completed_configurations_].target_on_stop.provider; - - RCLCPP_INFO(this->get_logger(), "--> on stop capability : %s from provider : %s", request_configure->target_on_stop.capability.c_str(), - request_configure->target_on_stop.provider.c_str()); } else { @@ -622,13 +640,10 @@ class CapabilitiesFabric : public rclcpp::Node } if (xml_parser::convert_to_string(capabilities[completed_configurations_].target_on_success.parameters, - request_configure->target_on_success.parameters)) + request_configure->target_on_success.parameters)) { request_configure->target_on_success.capability = capabilities[completed_configurations_].target_on_success.runner; request_configure->target_on_success.provider = capabilities[completed_configurations_].target_on_success.provider; - - RCLCPP_INFO(this->get_logger(), "--> on success capability : %s from provider : %s", request_configure->target_on_success.capability.c_str(), - request_configure->target_on_success.provider.c_str()); } else { @@ -637,13 +652,10 @@ class CapabilitiesFabric : public rclcpp::Node } if (xml_parser::convert_to_string(capabilities[completed_configurations_].target_on_failure.parameters, - request_configure->target_on_failure.parameters)) + request_configure->target_on_failure.parameters)) { request_configure->target_on_failure.capability = capabilities[completed_configurations_].target_on_failure.runner; request_configure->target_on_failure.provider = capabilities[completed_configurations_].target_on_failure.provider; - - RCLCPP_INFO(this->get_logger(), "--> on failure capability : %s from provider : %s", request_configure->target_on_failure.capability.c_str(), - request_configure->target_on_failure.provider.c_str()); } else { @@ -655,11 +667,11 @@ class CapabilitiesFabric : public rclcpp::Node // send the request auto result_future = conf_capability_client_->async_send_request( - request_configure, [this, goal_handle, source_capability](ConfigureCapabilityClient::SharedFuture future) { + request_configure, [this, source_capability](ConfigureCapabilityClient::SharedFuture future) { if (!future.valid()) { status = "Failed to configure capability :" + source_capability + ". Server execution cancelled"; - print_and_result(goal_handle, status, false); + process_result(status); return; } @@ -669,18 +681,18 @@ class CapabilitiesFabric : public rclcpp::Node status = std::to_string(completed_configurations_) + "/" + std::to_string(expected_configurations_) + " : Successfully configured capability : " + source_capability; - print_and_feedback(goal_handle, status, true); + process_feedback(status); // Check if all expected calls are completed before calling verify_plan if (completed_configurations_ == expected_configurations_) { - print_and_feedback(goal_handle, "All requested capabilities have been configured. Triggering the first capability", true); + process_feedback("All requested capabilities have been configured. Triggering the first capability", true); - trigger_first_node(goal_handle); + trigger_first_node(); } else { - configure_capabilities(connection_map, goal_handle); + configure_capabilities(connection_map); } }); } @@ -688,7 +700,7 @@ class CapabilitiesFabric : public rclcpp::Node /** * @brief Trigger the first node */ - void trigger_first_node(const std::shared_ptr goal_handle) + void trigger_first_node() { auto request_trigger = std::make_shared(); @@ -699,20 +711,20 @@ class CapabilitiesFabric : public rclcpp::Node // send the request auto result_future = - trig_capability_client_->async_send_request(request_trigger, [this, goal_handle](TriggerCapabilityClient::SharedFuture future) { + trig_capability_client_->async_send_request(request_trigger, [this](TriggerCapabilityClient::SharedFuture future) { if (!future.valid()) { status = "Failed to trigger capability " + connection_map[0].source.runner; - print_and_result(goal_handle, status, false); + process_result(status); return; } auto response = future.get(); status = "Successfully triggered capability " + connection_map[0].source.runner; - print_and_feedback(goal_handle, status, false); + process_feedback(status); - print_and_result(goal_handle, "Successfully launched capabilities2 fabric", true); + process_result("Successfully launched capabilities2 fabric", true); }); } @@ -721,40 +733,50 @@ class CapabilitiesFabric : public rclcpp::Node while (wait_for_logic) { std::string failed = service_name + " not available"; - RCLCPP_ERROR(this->get_logger(), failed.c_str()); + status_->error(failed); rclcpp::shutdown(); return; } std::string success = service_name + " connected"; - RCLCPP_INFO(this->get_logger(), success.c_str()); + status_->info(success); } - void print_and_feedback(const std::shared_ptr goal_handle, const std::string& text, bool newline) + /** + * @brief publishers feedback message and status message + * + * @param text content of the feedback message and status message + * @param newline whether to include a newline before the message + */ + void process_feedback(const std::string& text, bool newline = false) { feedback_msg->progress = text; - goal_handle->publish_feedback(feedback_msg); + goal_handle_->publish_feedback(feedback_msg); - if (newline) - RCLCPP_INFO(this->get_logger(), ""); - - RCLCPP_INFO(this->get_logger(), feedback_msg->progress.c_str()); + status_->info(text, newline); } - void print_and_result(const std::shared_ptr goal_handle, const std::string& text, bool success) + /** + * @brief publishers result message and status message + * + * @param text content of the feedback message and status message + * @param success whether the action succeeded + * @param newline whether to include a newline before the message + */ + void process_result(const std::string& text, bool success = false, bool newline = false) { result_msg->success = success; result_msg->message = text; if (success) { - goal_handle->succeed(result_msg); - RCLCPP_INFO(this->get_logger(), result_msg->message.c_str()); + goal_handle_->succeed(result_msg); + status_->info(text, newline); } else { - goal_handle->abort(result_msg); - RCLCPP_ERROR(this->get_logger(), result_msg->message.c_str()); + goal_handle_->abort(result_msg); + status_->error(text, newline); } } @@ -783,9 +805,12 @@ class CapabilitiesFabric : public rclcpp::Node /** Bond id */ std::string bond_id_; - /** Bond between capabilities server and this client */ + /** Manages bond between capabilities server and this client */ std::shared_ptr bond_client_; + /** Handles status message sending and printing to logging */ + std::shared_ptr status_; + /** XML Document */ tinyxml2::XMLDocument document; @@ -817,7 +842,7 @@ class CapabilitiesFabric : public rclcpp::Node std::shared_ptr> planner_server_; /** action server goal handle*/ - std::shared_ptr goal_handle; + std::shared_ptr goal_handle_; /** Get interfaces from capabilities server */ GetInterfacesClient::SharedPtr get_interfaces_client_; diff --git a/capabilities2_fabric/include/capabilities2_fabric/fabric_client.hpp b/capabilities2_fabric/include/capabilities2_fabric/fabric_client.hpp index a4d9a49..bef0f9d 100644 --- a/capabilities2_fabric/include/capabilities2_fabric/fabric_client.hpp +++ b/capabilities2_fabric/include/capabilities2_fabric/fabric_client.hpp @@ -10,7 +10,7 @@ #include #include -#include +#include #include /** diff --git a/capabilities2_fabric/include/capabilities2_fabric/utils/xml_parser.hpp b/capabilities2_fabric/include/capabilities2_fabric/utils/xml_parser.hpp deleted file mode 100644 index c9085dd..0000000 --- a/capabilities2_fabric/include/capabilities2_fabric/utils/xml_parser.hpp +++ /dev/null @@ -1,289 +0,0 @@ -#include -#include -#include -#include -#include - -namespace xml_parser -{ -/** - * @brief extract elements related plan - * - * @param document XML document to extract plan from - * - * @return plan in the form of tinyxml2::XMLElement* - */ -tinyxml2::XMLElement* get_plan(tinyxml2::XMLDocument& document) -{ - return document.FirstChildElement("Plan")->FirstChildElement(); -} - -/** - * @brief search a string in a vector of strings - * - * @param list vector of strings to be searched - * @param value string to be searched in the vector - * - * @return `true` if value is found in list and `false` otherwise - */ -bool search(std::vector list, std::string value) -{ - return (std::find(list.begin(), list.end(), value) != list.end()); -} - -/** - * @brief check if the element in question is the last in its level - * - * @param element element to be checked - * - * @return `true` if last and `false` otherwise - */ -bool isLastElement(tinyxml2::XMLElement* element) -{ - return (element == element->Parent()->LastChildElement()); -} - -/** - * @brief check if the xml document has valid plan tags - * - * @param document XML document in question - * - * @return `true` if its valid and `false` otherwise - */ -bool check_plan_tag(tinyxml2::XMLDocument& document) -{ - std::string plan_tag(document.FirstChildElement()->Name()); - - if (plan_tag == "Plan") - return true; - else - return false; -} - -/** - * @brief convert XMLElement to std::string - * - * @param element XMLElement element to be converted - * @param paramters parameter to hold std::string - * - * @return `true` if element is not nullptr and conversion successful, `false` if element is nullptr - */ -bool convert_to_string(tinyxml2::XMLElement* element, std::string& parameters) -{ - if (element) - { - tinyxml2::XMLPrinter printer; - element->Accept(&printer); - parameters = printer.CStr(); - return true; - } - else - { - parameters = ""; - return false; - } -} - -/** - * @brief convert XMLDocument to std::string - * - * @param document element to be converted - * - * @return std::string converted document - */ -void convert_to_string(tinyxml2::XMLDocument& document_xml, std::string& document_string) -{ - tinyxml2::XMLPrinter printer; - document_xml.Print(&printer); - document_string = printer.CStr(); -} - -/** - * @brief check the plan for invalid/unsupported control and event tags - * uses recursive approach to go through the plan - * - * @param element XML Element to be evaluated - * @param events list containing valid event tags - * @param providers list containing providers - * @param control list containing valid control tags - * @param rejected list containing invalid tags - * - * @return `true` if element valid and supported and `false` otherwise - */ -bool check_tags(rclcpp::Logger logger, tinyxml2::XMLElement* element, std::vector& events, std::vector& providers, - std::vector& control, std::vector& rejected) -{ - const char* name; - const char* provider; - - std::string parameter_string; - convert_to_string(element, parameter_string); - - element->QueryStringAttribute("name", &name); - element->QueryStringAttribute("provider", &provider); - - std::string nametag; - std::string providertag; - - std::string typetag(element->Name()); - - if (name) - nametag = name; - else - nametag = ""; - - if (provider) - providertag = provider; - else - providertag = ""; - - bool hasChildren = !element->NoChildren(); - bool hasSiblings = !xml_parser::isLastElement(element); - bool foundInControl = xml_parser::search(control, nametag); - bool foundInEvents = xml_parser::search(events, nametag); - bool foundInProviders = xml_parser::search(providers, providertag); - bool returnValue = true; - - if (typetag == "Control") - { - if (!foundInControl) - { - RCLCPP_ERROR(logger, "Control tag '%s' not available in the valid list", nametag.c_str()); - rejected.push_back(parameter_string); - return false; - } - - if (hasChildren) - returnValue &= xml_parser::check_tags(logger, element->FirstChildElement(), events, providers, control, rejected); - - if (hasSiblings) - returnValue &= xml_parser::check_tags(logger, element->NextSiblingElement(), events, providers, control, rejected); - } - else if (typetag == "Event") - { - if (!foundInEvents || !foundInProviders) - { - RCLCPP_ERROR(logger, "Event tag name '%s' or provider '%s' not available in the valid list", nametag.c_str(), providertag.c_str()); - rejected.push_back(parameter_string); - return false; - } - - if (hasSiblings) - returnValue &= xml_parser::check_tags(logger, element->NextSiblingElement(), events, providers, control, rejected); - } - else - { - RCLCPP_ERROR(logger, "xml element is not valid", parameter_string.c_str()); - rejected.push_back(parameter_string); - return false; - } - - return returnValue; -} - -/** - * @brief Returns control xml tags supported in extract_connections method - * - */ -std::vector get_control_list() -{ - std::vector tag_list; - - tag_list.push_back("sequential"); - tag_list.push_back("parallel"); - tag_list.push_back("recovery"); - - return tag_list; -} - -/** - * @brief parse through the plan and extract the connections - * - * @param element XML Element to be evaluated - * @param connections std::map containing extracted connections - * @param connection_id numerical id of the connection - * @param connection_type the type of connection - */ -void extract_connections(tinyxml2::XMLElement* element, std::map& connections, int connection_id = 0, - capabilities2::connection_type_t connection_type = capabilities2::connection_type_t::ON_SUCCESS) -{ - int predecessor_id; - - const char* name; - const char* provider; - - element->QueryStringAttribute("name", &name); - element->QueryStringAttribute("provider", &provider); - - std::string typetag(element->Name()); - - std::string nametag; - std::string providertag; - - if (name) - nametag = name; - else - nametag = ""; - - if (provider) - providertag = provider; - else - providertag = ""; - - bool hasChildren = !element->NoChildren(); - bool hasSiblings = !xml_parser::isLastElement(element); - - if ((typetag == "Control") and (nametag == "sequential")) - { - if (hasChildren) - xml_parser::extract_connections(element->FirstChildElement(), connections, connection_id, capabilities2::connection_type_t::ON_SUCCESS); - - if (hasSiblings) - xml_parser::extract_connections(element->NextSiblingElement(), connections, connection_id, connection_type); - } - else if ((typetag == "Control") and (nametag == "parallel")) - { - if (hasChildren) - xml_parser::extract_connections(element->FirstChildElement(), connections, connection_id, capabilities2::connection_type_t::ON_START); - - if (hasSiblings) - xml_parser::extract_connections(element->NextSiblingElement(), connections, connection_id, connection_type); - } - else if ((typetag == "Control") and (nametag == "recovery")) - { - if (hasChildren) - xml_parser::extract_connections(element->FirstChildElement(), connections, connection_id, capabilities2::connection_type_t::ON_FAILURE); - - if (hasSiblings) - xml_parser::extract_connections(element->NextSiblingElement(), connections, connection_id, connection_type); - } - else if (typetag == "Event") - { - capabilities2::node_t node; - - node.source.runner = nametag; - node.source.provider = providertag; - node.source.parameters = element; - - predecessor_id = connection_id - 1; - - while (connections.count(connection_id) > 0) - connection_id += 1; - - connections[connection_id] = node; - - if ((connection_type == capabilities2::connection_type_t::ON_SUCCESS) and (connection_id != 0)) - connections[predecessor_id].target_on_success = connections[connection_id].source; - - else if ((connection_type == capabilities2::connection_type_t::ON_START) and (connection_id != 0)) - connections[predecessor_id].target_on_start = connections[connection_id].source; - - else if ((connection_type == capabilities2::connection_type_t::ON_FAILURE) and (connection_id != 0)) - connections[predecessor_id].target_on_failure = connections[connection_id].source; - - if (hasSiblings) - xml_parser::extract_connections(element->NextSiblingElement(), connections, connection_id + 1, connection_type); - } -} - -} // namespace xml_parser diff --git a/capabilities2_fabric/launch/fabric.launch.py b/capabilities2_fabric/launch/fabric.launch.py index d0e6dfc..71493f1 100644 --- a/capabilities2_fabric/launch/fabric.launch.py +++ b/capabilities2_fabric/launch/fabric.launch.py @@ -15,9 +15,9 @@ def generate_launch_description(): LaunchDescription: The launch description for capabilities2 executor """ # load config file - executor_config = os.path.join(get_package_share_directory('capabilities2_fabric'), 'config', 'fabric.yaml') + fabric_config = os.path.join(get_package_share_directory('capabilities2_fabric'), 'config', 'fabric.yaml') - executor = Node( + capabilities2_fabric = Node( package='capabilities2_fabric', namespace='', executable='capabilities2_fabric', @@ -25,17 +25,16 @@ def generate_launch_description(): output='screen' ) - executor_file = Node( + fabric_client = Node( package='capabilities2_fabric', namespace='', executable='fabric_client', name='fabric_client', - parameters=[executor_config], + parameters=[fabric_config], output='screen' ) - # return return LaunchDescription([ - executor, - executor_file + capabilities2_fabric, + fabric_client ]) diff --git a/capabilities2_fabric/package.xml b/capabilities2_fabric/package.xml index 43af93a..66543ff 100644 --- a/capabilities2_fabric/package.xml +++ b/capabilities2_fabric/package.xml @@ -14,6 +14,7 @@ capabilities2_msgs rclcpp_action backward_ros + capabilities2_utils tinyxml2 diff --git a/capabilities2_msgs/CMakeLists.txt b/capabilities2_msgs/CMakeLists.txt index 58edbe3..ca75c2e 100644 --- a/capabilities2_msgs/CMakeLists.txt +++ b/capabilities2_msgs/CMakeLists.txt @@ -24,6 +24,7 @@ set(msg_files "msg/NaturalCapability.msg" "msg/CapabilityCommand.msg" "msg/CapabilityResponse.msg" + "msg/Status.msg" ) set(srv_files @@ -43,6 +44,8 @@ set(srv_files "srv/ConfigureCapability.srv" "srv/TriggerCapability.srv" "srv/Launch.srv" + "srv/GetFabricStatus.srv" + "srv/SetFabricPlan.srv" ) set(action_files diff --git a/capabilities2_msgs/msg/Status.msg b/capabilities2_msgs/msg/Status.msg new file mode 100644 index 0000000..7d88ea3 --- /dev/null +++ b/capabilities2_msgs/msg/Status.msg @@ -0,0 +1,7 @@ +std_msgs/Header header +string origin_node +string origin_runner +string text +bool failure +bool is_failed_element +string failed_element \ No newline at end of file diff --git a/capabilities2_msgs/srv/GetFabricStatus.srv b/capabilities2_msgs/srv/GetFabricStatus.srv new file mode 100644 index 0000000..8706a0b --- /dev/null +++ b/capabilities2_msgs/srv/GetFabricStatus.srv @@ -0,0 +1,7 @@ +std_msgs/Header header +--- +std_msgs/Header header +uint8 status +uint8 FABRIC_IDLE=0 +uint8 FABRIC_RUNNING=1 +uint8 FABRIC_FAILED=2 \ No newline at end of file diff --git a/capabilities2_msgs/srv/SetFabricPlan.srv b/capabilities2_msgs/srv/SetFabricPlan.srv new file mode 100644 index 0000000..49d3223 --- /dev/null +++ b/capabilities2_msgs/srv/SetFabricPlan.srv @@ -0,0 +1,5 @@ +std_msgs/Header header +string plan +--- +std_msgs/Header header +bool success \ No newline at end of file diff --git a/capabilities2_server/CMakeLists.txt b/capabilities2_server/CMakeLists.txt index 3eb130c..d36df9b 100644 --- a/capabilities2_server/CMakeLists.txt +++ b/capabilities2_server/CMakeLists.txt @@ -19,6 +19,7 @@ find_package(pluginlib REQUIRED) find_package(rclcpp_components REQUIRED) find_package(capabilities2_msgs REQUIRED) find_package(capabilities2_runner REQUIRED) +find_package(capabilities2_utils REQUIRED) find_package(backward_ros REQUIRED) find_package(PkgConfig) @@ -68,6 +69,7 @@ ament_target_dependencies(${PROJECT_NAME} SQLite3 yaml-cpp UUID + capabilities2_utils ) rclcpp_components_register_node(${PROJECT_NAME} @@ -106,6 +108,7 @@ ament_target_dependencies(${PROJECT_NAME}_node capabilities2_msgs capabilities2_runner SQLite3 + capabilities2_utils yaml-cpp UUID ) diff --git a/capabilities2_server/include/capabilities2_server/capabilities_db.hpp b/capabilities2_server/include/capabilities2_server/capabilities_db.hpp index 2a724dd..e5ff407 100644 --- a/capabilities2_server/include/capabilities2_server/capabilities_db.hpp +++ b/capabilities2_server/include/capabilities2_server/capabilities_db.hpp @@ -13,7 +13,8 @@ #include #include #include -#include + +#include namespace capabilities2_server { diff --git a/capabilities2_server/include/capabilities2_server/models/header.hpp b/capabilities2_server/include/capabilities2_server/models/header.hpp index 3451957..0faa212 100644 --- a/capabilities2_server/include/capabilities2_server/models/header.hpp +++ b/capabilities2_server/include/capabilities2_server/models/header.hpp @@ -2,7 +2,7 @@ #include #include -#include +#include namespace capabilities2_server { diff --git a/capabilities2_server/include/capabilities2_server/models/interface.hpp b/capabilities2_server/include/capabilities2_server/models/interface.hpp index 69fa8ab..0976a31 100644 --- a/capabilities2_server/include/capabilities2_server/models/interface.hpp +++ b/capabilities2_server/include/capabilities2_server/models/interface.hpp @@ -5,7 +5,7 @@ #include #include #include -#include +#include namespace capabilities2_server { diff --git a/capabilities2_server/include/capabilities2_server/models/provider.hpp b/capabilities2_server/include/capabilities2_server/models/provider.hpp index 91269fa..e436493 100644 --- a/capabilities2_server/include/capabilities2_server/models/provider.hpp +++ b/capabilities2_server/include/capabilities2_server/models/provider.hpp @@ -6,7 +6,7 @@ #include #include #include -#include +#include namespace capabilities2_server { diff --git a/capabilities2_server/include/capabilities2_server/models/semantic_interface.hpp b/capabilities2_server/include/capabilities2_server/models/semantic_interface.hpp index b479aea..864a564 100644 --- a/capabilities2_server/include/capabilities2_server/models/semantic_interface.hpp +++ b/capabilities2_server/include/capabilities2_server/models/semantic_interface.hpp @@ -4,7 +4,7 @@ #include #include #include -#include +#include namespace capabilities2_server { diff --git a/capabilities2_server/package.xml b/capabilities2_server/package.xml index e9fb76d..95062d6 100644 --- a/capabilities2_server/package.xml +++ b/capabilities2_server/package.xml @@ -22,6 +22,7 @@ rclcpp_components capabilities2_msgs capabilities2_runner + capabilities2_utils yaml-cpp diff --git a/capabilities2_utils/CMakeLists.txt b/capabilities2_utils/CMakeLists.txt new file mode 100644 index 0000000..6c80272 --- /dev/null +++ b/capabilities2_utils/CMakeLists.txt @@ -0,0 +1,24 @@ +cmake_minimum_required(VERSION 3.8) +project(capabilities2_utils) + +# Default to C++17 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(bondcpp REQUIRED) + +install(DIRECTORY include/ + DESTINATION include +) + +ament_export_include_directories(include) +ament_export_libraries(${PROJECT_NAME}) + +ament_package() diff --git a/capabilities2_fabric/include/capabilities2_fabric/utils/bond_client.hpp b/capabilities2_utils/include/capabilities2_utils/bond_client.hpp similarity index 99% rename from capabilities2_fabric/include/capabilities2_fabric/utils/bond_client.hpp rename to capabilities2_utils/include/capabilities2_utils/bond_client.hpp index b1f286a..401391e 100644 --- a/capabilities2_fabric/include/capabilities2_fabric/utils/bond_client.hpp +++ b/capabilities2_utils/include/capabilities2_utils/bond_client.hpp @@ -1,3 +1,4 @@ +#pragma once #include #include diff --git a/capabilities2_fabric/include/capabilities2_fabric/utils/connection.hpp b/capabilities2_utils/include/capabilities2_utils/connection.hpp similarity index 97% rename from capabilities2_fabric/include/capabilities2_fabric/utils/connection.hpp rename to capabilities2_utils/include/capabilities2_utils/connection.hpp index 4840bc8..8aa94f4 100644 --- a/capabilities2_fabric/include/capabilities2_fabric/utils/connection.hpp +++ b/capabilities2_utils/include/capabilities2_utils/connection.hpp @@ -1,3 +1,4 @@ +#pragma once #include #include diff --git a/capabilities2_server/include/capabilities2_server/sanitizer/sql_safe.hpp b/capabilities2_utils/include/capabilities2_utils/sql_safe.hpp similarity index 100% rename from capabilities2_server/include/capabilities2_server/sanitizer/sql_safe.hpp rename to capabilities2_utils/include/capabilities2_utils/sql_safe.hpp diff --git a/capabilities2_utils/include/capabilities2_utils/status_client.hpp b/capabilities2_utils/include/capabilities2_utils/status_client.hpp new file mode 100644 index 0000000..c9afd96 --- /dev/null +++ b/capabilities2_utils/include/capabilities2_utils/status_client.hpp @@ -0,0 +1,110 @@ +#pragma once +#include +#include + +class StatusClient +{ +public: + using Status = capabilities2_msgs::msg::Status; + + /** + * @brief Construct a new Status Client object + * + * @param node Pointer to the node + * @param node_name node name to be used for status message + * @param topic_name topic name to publish the message + */ + StatusClient(rclcpp::Node::SharedPtr node, const std::string& node_name,const std::string& topic_name) + { + node_ = node; + node_name_ = node_name; + status_publisher_ = node_->create_publisher(topic_name, 10); + } + + /** + * @brief publishes status information to the given topic and prints to the logging as info + * + * @param text Text to be published + * @param newline whether to add new line to the ROS_INFO print + */ + void info(const std::string &text, bool newline = false) + { + auto status_msg = Status(); + status_msg.header.stamp = node_->now(); + status_msg.origin_node = node_name_; + status_msg.text = text; + status_msg.failure = false; + + if (newline) + RCLCPP_INFO(node_->get_logger(), ""); + + RCLCPP_INFO(node_->get_logger(), status_msg.text.c_str()); + + status_publisher_->publish(status_msg); + } + + /** + * @brief publishes status information to the given topic and prints to the logging as error + * + * @param text Text to be published + * @param newline whether to add new line to the ROS_INFO print + */ + void error(const std::string &text, bool newline = false) + { + auto status_msg = Status(); + status_msg.header.stamp = node_->now(); + status_msg.origin_node = node_name_; + status_msg.text = text; + status_msg.failure = true; + status_msg.is_failed_element = false; + + if (newline) + RCLCPP_ERROR(node_->get_logger(), ""); + + RCLCPP_ERROR(node_->get_logger(), status_msg.text.c_str()); + + status_publisher_->publish(status_msg); + } + + /** + * @brief publishes status information to the given topic and prints to the logging as error + * + * @param text Text to be published + * @param newline whether to add new line to the ROS_INFO print + */ + void error_element(const std::string &element) + { + auto status_msg = Status(); + status_msg.header.stamp = node_->now(); + status_msg.origin_node = node_name_; + status_msg.text = "Failed element"; + status_msg.failure = true; + status_msg.is_failed_element = true; + status_msg.failed_element = element; + + std::string msg = status_msg.text + " : " + status_msg.failed_element; + + RCLCPP_ERROR(node_->get_logger(), msg.c_str()); + + status_publisher_->publish(status_msg); + } + +protected: + /** + * @brief Node pointer to access logging interface + * + */ + rclcpp::Node::SharedPtr node_; + + /** + * @brief publisher to publish execution status + * + */ + rclcpp::Publisher::SharedPtr status_publisher_; + + /** + * @brief Node name + * + */ + std::string node_name_; +}; \ No newline at end of file diff --git a/capabilities2_utils/include/capabilities2_utils/xml_parser.hpp b/capabilities2_utils/include/capabilities2_utils/xml_parser.hpp new file mode 100644 index 0000000..991b5eb --- /dev/null +++ b/capabilities2_utils/include/capabilities2_utils/xml_parser.hpp @@ -0,0 +1,295 @@ +#pragma once +#include +#include +#include +#include +#include +#include + +namespace xml_parser +{ + /** + * @brief extract elements related plan + * + * @param document XML document to extract plan from + * + * @return plan in the form of tinyxml2::XMLElement* + */ + tinyxml2::XMLElement *get_plan(tinyxml2::XMLDocument &document) + { + return document.FirstChildElement("Plan")->FirstChildElement(); + } + + /** + * @brief search a string in a vector of strings + * + * @param list vector of strings to be searched + * @param value string to be searched in the vector + * + * @return `true` if value is found in list and `false` otherwise + */ + bool search(std::vector list, std::string value) + { + return (std::find(list.begin(), list.end(), value) != list.end()); + } + + /** + * @brief check if the element in question is the last in its level + * + * @param element element to be checked + * + * @return `true` if last and `false` otherwise + */ + bool isLastElement(tinyxml2::XMLElement *element) + { + return (element == element->Parent()->LastChildElement()); + } + + /** + * @brief check if the xml document has valid plan tags + * + * @param document XML document in question + * + * @return `true` if its valid and `false` otherwise + */ + bool check_plan_tag(tinyxml2::XMLDocument &document) + { + std::string plan_tag(document.FirstChildElement()->Name()); + + if (plan_tag == "Plan") + return true; + else + return false; + } + + /** + * @brief convert XMLElement to std::string + * + * @param element XMLElement element to be converted + * @param paramters parameter to hold std::string + * + * @return `true` if element is not nullptr and conversion successful, `false` if element is nullptr + */ + bool convert_to_string(tinyxml2::XMLElement *element, std::string ¶meters) + { + if (element) + { + tinyxml2::XMLPrinter printer; + element->Accept(&printer); + parameters = printer.CStr(); + return true; + } + else + { + parameters = ""; + return false; + } + } + + /** + * @brief convert XMLDocument to std::string + * + * @param document element to be converted + * + * @return std::string converted document + */ + void convert_to_string(tinyxml2::XMLDocument &document_xml, std::string &document_string) + { + tinyxml2::XMLPrinter printer; + document_xml.Print(&printer); + document_string = printer.CStr(); + } + + /** + * @brief check the plan for invalid/unsupported control and event tags + * uses recursive approach to go through the plan + * + * @param status StatusClient used for logging and status publishing + * @param element XML Element to be evaluated + * @param events list containing valid event tags + * @param providers list containing providers + * @param control list containing valid control tags + * @param rejected list containing invalid tags + * + * @return `true` if element valid and supported and `false` otherwise + */ + bool check_tags(const std::shared_ptr status, tinyxml2::XMLElement *element, std::vector &events, std::vector &providers, + std::vector &control, std::vector &rejected) + { + const char *name; + const char *provider; + + std::string parameter_string; + convert_to_string(element, parameter_string); + + element->QueryStringAttribute("name", &name); + element->QueryStringAttribute("provider", &provider); + + std::string nametag; + std::string providertag; + + std::string typetag(element->Name()); + + if (name) + nametag = name; + else + nametag = ""; + + if (provider) + providertag = provider; + else + providertag = ""; + + bool hasChildren = !element->NoChildren(); + bool hasSiblings = !xml_parser::isLastElement(element); + bool foundInControl = xml_parser::search(control, nametag); + bool foundInEvents = xml_parser::search(events, nametag); + bool foundInProviders = xml_parser::search(providers, providertag); + bool returnValue = true; + + if (typetag == "Control") + { + if (!foundInControl) + { + std::string msg = "Control tag '" + nametag + "' not available in the valid list"; + status->error(msg); + rejected.push_back(parameter_string); + return false; + } + + if (hasChildren) + returnValue &= xml_parser::check_tags(status, element->FirstChildElement(), events, providers, control, rejected); + + if (hasSiblings) + returnValue &= xml_parser::check_tags(status, element->NextSiblingElement(), events, providers, control, rejected); + } + else if (typetag == "Event") + { + if (!foundInEvents || !foundInProviders) + { + std::string msg = "Event tag name '" + nametag + "' or provider '" + providertag + "' not available in the valid list"; + status->error(msg); + rejected.push_back(parameter_string); + return false; + } + + if (hasSiblings) + returnValue &= xml_parser::check_tags(status, element->NextSiblingElement(), events, providers, control, rejected); + } + else + { + std::string msg = "XML element is not valid :" + parameter_string; + status->error(msg); + rejected.push_back(parameter_string); + return false; + } + + return returnValue; + } + + /** + * @brief Returns control xml tags supported in extract_connections method + * + */ + std::vector get_control_list() + { + std::vector tag_list; + + tag_list.push_back("sequential"); + tag_list.push_back("parallel"); + tag_list.push_back("recovery"); + + return tag_list; + } + + /** + * @brief parse through the plan and extract the connections + * + * @param element XML Element to be evaluated + * @param connections std::map containing extracted connections + * @param connection_id numerical id of the connection + * @param connection_type the type of connection + */ + void extract_connections(tinyxml2::XMLElement *element, std::map &connections, int connection_id = 0, + capabilities2::connection_type_t connection_type = capabilities2::connection_type_t::ON_SUCCESS) + { + int predecessor_id; + + const char *name; + const char *provider; + + element->QueryStringAttribute("name", &name); + element->QueryStringAttribute("provider", &provider); + + std::string typetag(element->Name()); + + std::string nametag; + std::string providertag; + + if (name) + nametag = name; + else + nametag = ""; + + if (provider) + providertag = provider; + else + providertag = ""; + + bool hasChildren = !element->NoChildren(); + bool hasSiblings = !xml_parser::isLastElement(element); + + if ((typetag == "Control") and (nametag == "sequential")) + { + if (hasChildren) + xml_parser::extract_connections(element->FirstChildElement(), connections, connection_id, capabilities2::connection_type_t::ON_SUCCESS); + + if (hasSiblings) + xml_parser::extract_connections(element->NextSiblingElement(), connections, connection_id, connection_type); + } + else if ((typetag == "Control") and (nametag == "parallel")) + { + if (hasChildren) + xml_parser::extract_connections(element->FirstChildElement(), connections, connection_id, capabilities2::connection_type_t::ON_START); + + if (hasSiblings) + xml_parser::extract_connections(element->NextSiblingElement(), connections, connection_id, connection_type); + } + else if ((typetag == "Control") and (nametag == "recovery")) + { + if (hasChildren) + xml_parser::extract_connections(element->FirstChildElement(), connections, connection_id, capabilities2::connection_type_t::ON_FAILURE); + + if (hasSiblings) + xml_parser::extract_connections(element->NextSiblingElement(), connections, connection_id, connection_type); + } + else if (typetag == "Event") + { + capabilities2::node_t node; + + node.source.runner = nametag; + node.source.provider = providertag; + node.source.parameters = element; + + predecessor_id = connection_id - 1; + + while (connections.count(connection_id) > 0) + connection_id += 1; + + connections[connection_id] = node; + + if ((connection_type == capabilities2::connection_type_t::ON_SUCCESS) and (connection_id != 0)) + connections[predecessor_id].target_on_success = connections[connection_id].source; + + else if ((connection_type == capabilities2::connection_type_t::ON_START) and (connection_id != 0)) + connections[predecessor_id].target_on_start = connections[connection_id].source; + + else if ((connection_type == capabilities2::connection_type_t::ON_FAILURE) and (connection_id != 0)) + connections[predecessor_id].target_on_failure = connections[connection_id].source; + + if (hasSiblings) + xml_parser::extract_connections(element->NextSiblingElement(), connections, connection_id + 1, connection_type); + } + } + +} // namespace xml_parser diff --git a/capabilities2_utils/package.xml b/capabilities2_utils/package.xml new file mode 100644 index 0000000..bd910d3 --- /dev/null +++ b/capabilities2_utils/package.xml @@ -0,0 +1,21 @@ + + + + capabilities2_utils + 0.0.0 + TODO: Package description + kalana + TODO: License declaration + + ament_cmake + + ament_lint_auto + ament_lint_common + + rclcpp + bondcpp + + + ament_cmake + + From cb7aeb17be109afd84bf80f18bc20562eefd800e Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Tue, 28 Jan 2025 01:37:14 +1100 Subject: [PATCH 078/133] added server interface for interacting with capabilities2_fabric via fabric_client --- .../capabilities_fabric.hpp | 36 ++- .../capabilities2_fabric/fabric_client.hpp | 212 ++++++++++++++---- capabilities2_msgs/CMakeLists.txt | 1 + capabilities2_msgs/srv/CancelFabricPlan.srv | 4 + capabilities2_msgs/srv/GetFabricStatus.srv | 6 +- 5 files changed, 198 insertions(+), 61 deletions(-) create mode 100644 capabilities2_msgs/srv/CancelFabricPlan.srv diff --git a/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric.hpp b/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric.hpp index bc16c7d..11978c3 100644 --- a/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric.hpp +++ b/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric.hpp @@ -387,16 +387,13 @@ class CapabilitiesFabric : public rclcpp::Node result->success = false; result->message = "Plan verification failed. There are mismatched events"; - process_feedback(result->message); - for (const auto& rejected_element : rejected_list) { - status_->error_element(rejected_element); result->failed_elements.push_back(rejected_element); } - goal_handle_->abort(result); - status_->error("Server Execution Cancelled"); + + process_feedback(result->message); } else { @@ -666,8 +663,8 @@ class CapabilitiesFabric : public rclcpp::Node std::string source_capability = capabilities[completed_configurations_].source.runner; // send the request - auto result_future = conf_capability_client_->async_send_request( - request_configure, [this, source_capability](ConfigureCapabilityClient::SharedFuture future) { + auto result_future = + conf_capability_client_->async_send_request(request_configure, [this, source_capability](ConfigureCapabilityClient::SharedFuture future) { if (!future.valid()) { status = "Failed to configure capability :" + source_capability + ". Server execution cancelled"; @@ -710,22 +707,21 @@ class CapabilitiesFabric : public rclcpp::Node request_trigger->parameters = parameter_string; // send the request - auto result_future = - trig_capability_client_->async_send_request(request_trigger, [this](TriggerCapabilityClient::SharedFuture future) { - if (!future.valid()) - { - status = "Failed to trigger capability " + connection_map[0].source.runner; - process_result(status); - return; - } + auto result_future = trig_capability_client_->async_send_request(request_trigger, [this](TriggerCapabilityClient::SharedFuture future) { + if (!future.valid()) + { + status = "Failed to trigger capability " + connection_map[0].source.runner; + process_result(status); + return; + } - auto response = future.get(); + auto response = future.get(); - status = "Successfully triggered capability " + connection_map[0].source.runner; - process_feedback(status); + status = "Successfully triggered capability " + connection_map[0].source.runner; + process_feedback(status); - process_result("Successfully launched capabilities2 fabric", true); - }); + process_result("Successfully launched capabilities2 fabric", true); + }); } void check_service(bool wait_for_logic, const std::string& service_name) diff --git a/capabilities2_fabric/include/capabilities2_fabric/fabric_client.hpp b/capabilities2_fabric/include/capabilities2_fabric/fabric_client.hpp index bef0f9d..46229b5 100644 --- a/capabilities2_fabric/include/capabilities2_fabric/fabric_client.hpp +++ b/capabilities2_fabric/include/capabilities2_fabric/fabric_client.hpp @@ -11,8 +11,14 @@ #include #include +#include + #include +#include +#include +#include + /** * @brief Capabilities Executor File Parser * @@ -23,6 +29,23 @@ class CapabilitiesFabricClient : public rclcpp::Node { public: + enum fabric_status + { + IDLE, + RUNNING, + CANCELED, + ABORTED, + FAILED, + SUCCEEDED + }; + + using Plan = capabilities2_msgs::action::Plan; + using GoalHandlePlan = rclcpp_action::ClientGoalHandle; + + using GetFabricStatus = capabilities2_msgs::srv::GetFabricStatus; + using SetFabricPlan = capabilities2_msgs::srv::SetFabricPlan; + using CancelFabricPlan = capabilities2_msgs::srv::CancelFabricPlan; + CapabilitiesFabricClient() : Node("Capabilities2_Fabric_Client") { declare_parameter("plan_file_path", "install/capabilities2_fabric/share/capabilities2_fabric/plans/default.xml"); @@ -35,104 +58,213 @@ class CapabilitiesFabricClient : public rclcpp::Node */ void initialize() { + fabric_state = fabric_status::IDLE; + + status_ = std::make_shared(shared_from_this(), "capabilities_fabric_client", "/status/capabilities_fabric_client"); + + status_server_ = + this->create_service("/capabilities_fabric/get_status", std::bind(&CapabilitiesFabricClient::getStatusCallback, this, + std::placeholders::_1, std::placeholders::_2)); + + plan_server_ = this->create_service( + "/capabilities_fabric/set_plan", std::bind(&CapabilitiesFabricClient::setPlanCallback, this, std::placeholders::_1, std::placeholders::_2)); + + cancel_server_ = + this->create_service("/capabilities_fabric/cancel_plan", std::bind(&CapabilitiesFabricClient::cancelPlanCallback, this, + std::placeholders::_1, std::placeholders::_2)); + // Create the action client for capabilities_fabric after the node is fully constructed - this->client_ptr_ = rclcpp_action::create_client(shared_from_this(), "/capabilities_fabric"); + this->planner_client_ = rclcpp_action::create_client(shared_from_this(), "/capabilities_fabric"); - if (!this->client_ptr_->wait_for_action_server(std::chrono::seconds(5))) + if (!this->planner_client_->wait_for_action_server(std::chrono::seconds(5))) { - RCLCPP_ERROR(this->get_logger(), "Action server not available after waiting"); + status_->error("Action server not available after waiting"); rclcpp::shutdown(); return; } - RCLCPP_INFO(this->get_logger(), "Sucessfully connected to the capabilities_fabric action server"); - - send_goal(); - } + status_->info("Sucessfully connected to the capabilities_fabric action server"); - void send_goal() - { // try to load the file tinyxml2::XMLError xml_status = document.LoadFile(plan_file_path.c_str()); // check if the file loading failed if (xml_status != tinyxml2::XMLError::XML_SUCCESS) { - RCLCPP_ERROR(this->get_logger(), "Error loading plan: %s, Error: %s", plan_file_path.c_str(), document.ErrorName()); + status = "Error loading plan: " + plan_file_path + ", Error: " + document.ErrorName(); + status_->error(status); rclcpp::shutdown(); } - RCLCPP_INFO(this->get_logger(), "Plan loaded from : %s", plan_file_path.c_str()); + status = "Plan loaded from : " + plan_file_path; + status_->info(status); + + send_goal(document); + } - auto goal_msg = capabilities2_msgs::action::Plan::Goal(); +private: + void send_goal(tinyxml2::XMLDocument& document_xml) + { + auto goal_msg = Plan::Goal(); - xml_parser::convert_to_string(document, goal_msg.plan); + xml_parser::convert_to_string(document_xml, goal_msg.plan); - RCLCPP_INFO(this->get_logger(), "Following plan was loaded :\n\n%s ", goal_msg.plan.c_str()); + status = "Following plan was loaded :\n\n " + goal_msg.plan; + status_->info(status); - RCLCPP_INFO(this->get_logger(), "Sending goal to the capabilities_fabric action server"); + status_->info("Sending goal to the capabilities_fabric action server"); - auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); + auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); // send goal options // goal response callback - send_goal_options.goal_response_callback = - [this](const rclcpp_action::ClientGoalHandle::SharedPtr& goal_handle) { - if (!goal_handle) - { - RCLCPP_ERROR(this->get_logger(), "Goal was rejected by server"); - } - else - { - RCLCPP_INFO(this->get_logger(), "Goal accepted by server, waiting for result"); - } - }; + send_goal_options.goal_response_callback = [this](const GoalHandlePlan::SharedPtr& goal_handle) { + if (!goal_handle) + { + status_->error("Goal was rejected by server"); + fabric_state = fabric_status::FAILED; + } + else + { + status_->info("Goal accepted by server, waiting for result"); + goal_handle_ = goal_handle; + fabric_state = fabric_status::RUNNING; + } + }; // result callback - send_goal_options.result_callback = [this](const rclcpp_action::ClientGoalHandle::WrappedResult& result) { + send_goal_options.result_callback = [this](const GoalHandlePlan::WrappedResult& result) { switch (result.code) { case rclcpp_action::ResultCode::SUCCEEDED: + fabric_state = fabric_status::SUCCEEDED; break; case rclcpp_action::ResultCode::ABORTED: - RCLCPP_ERROR(this->get_logger(), "Goal was aborted"); - return; + status_->error("Goal was aborted"); + fabric_state = fabric_status::ABORTED; + break; case rclcpp_action::ResultCode::CANCELED: - RCLCPP_ERROR(this->get_logger(), "Goal was canceled"); - return; + status_->error("Goal was canceled"); + fabric_state = fabric_status::CANCELED; + break; default: - RCLCPP_ERROR(this->get_logger(), "Unknown result code"); - return; + status_->error("Unknown result code"); + fabric_state = fabric_status::FAILED; + break; } if (result.result->success) { - RCLCPP_INFO(this->get_logger(), "Plan executed successfully"); + status_->info("Plan executed successfully"); } else { - RCLCPP_ERROR(this->get_logger(), "Plan failed to complete"); + status_->error("Plan failed to complete"); if (result.result->failed_elements.size() > 0) { - RCLCPP_ERROR(this->get_logger(), "Plan failed due to incompatible XMLElements in the plan"); + status_->error("Plan failed due to incompatible XMLElements in the plan"); for (const auto& failed_element : result.result->failed_elements) - RCLCPP_ERROR(this->get_logger(), "Failed Elements : %s", failed_element.c_str()); + status_->error_element(failed_element); } } }; - this->client_ptr_->async_send_goal(goal_msg, send_goal_options); + this->planner_client_->async_send_goal(goal_msg, send_goal_options); + } + + void getStatusCallback(const std::shared_ptr request, std::shared_ptr response) + { + if (fabric_state == fabric_status::IDLE) + { + response->status = GetFabricStatus::Response::FABRIC_IDLE; + } + else if (fabric_state == fabric_status::RUNNING) + { + response->status = GetFabricStatus::Response::FABRIC_RUNNING; + } + else if (fabric_state == fabric_status::CANCELED) + { + response->status = GetFabricStatus::Response::FABRIC_CANCELED; + } + else if (fabric_state == fabric_status::ABORTED) + { + response->status = GetFabricStatus::Response::FABRIC_ABORTED; + } + else if (fabric_state == fabric_status::FAILED) + { + response->status = GetFabricStatus::Response::FABRIC_FAILED; + } + else if (fabric_state == fabric_status::SUCCEEDED) + { + response->status = GetFabricStatus::Response::FABRIC_SUCCEEDED; + } + else + { + response->status = GetFabricStatus::Response::UNKNOWN; + } + } + + void setPlanCallback(const std::shared_ptr request, std::shared_ptr response) + { + status_->info("Received the request with a plan"); + + // try to parse the std::string plan from capabilities_msgs/Plan to the to a XMLDocument file + tinyxml2::XMLError xml_status = document.Parse(request->plan.c_str()); + + // check if the file parsing failed + if (xml_status != tinyxml2::XMLError::XML_SUCCESS) + { + status_->info("Parsing the plan from service request message failed"); + response->success = false; + } + + status_->info("Plan parsed and accepted"); + + response->success = true; + + send_goal(document); + } + + void cancelPlanCallback(const std::shared_ptr request, std::shared_ptr response) + { + if (fabric_state == fabric_status::RUNNING) + { + this->planner_client_->async_cancel_goal(goal_handle_); + } + + response->success = true; } private: /** File Path link */ std::string plan_file_path; + /** Status message */ + std::string status; + /** XML Document */ tinyxml2::XMLDocument document; /** action client */ - rclcpp_action::Client::SharedPtr client_ptr_; + rclcpp_action::Client::SharedPtr planner_client_; + + /** Handles status message sending and printing to logging */ + std::shared_ptr status_; + + /** Goal handle for action client control */ + GoalHandlePlan::SharedPtr goal_handle_; + + /** server to get the status of the capabilities2 fabric */ + rclcpp::Service::SharedPtr status_server_; + + /** server to set a new plan to the capabilities2 fabric */ + rclcpp::Service::SharedPtr plan_server_; + + /** server to cancel the current plan in the capabilities2 fabric */ + rclcpp::Service::SharedPtr cancel_server_; + + /** Status of the fabric */ + fabric_status fabric_state; }; \ No newline at end of file diff --git a/capabilities2_msgs/CMakeLists.txt b/capabilities2_msgs/CMakeLists.txt index ca75c2e..b50c9eb 100644 --- a/capabilities2_msgs/CMakeLists.txt +++ b/capabilities2_msgs/CMakeLists.txt @@ -46,6 +46,7 @@ set(srv_files "srv/Launch.srv" "srv/GetFabricStatus.srv" "srv/SetFabricPlan.srv" + "srv/CancelFabricPlan.srv" ) set(action_files diff --git a/capabilities2_msgs/srv/CancelFabricPlan.srv b/capabilities2_msgs/srv/CancelFabricPlan.srv new file mode 100644 index 0000000..25a4b22 --- /dev/null +++ b/capabilities2_msgs/srv/CancelFabricPlan.srv @@ -0,0 +1,4 @@ +std_msgs/Header header +--- +std_msgs/Header header +bool success \ No newline at end of file diff --git a/capabilities2_msgs/srv/GetFabricStatus.srv b/capabilities2_msgs/srv/GetFabricStatus.srv index 8706a0b..6e12c28 100644 --- a/capabilities2_msgs/srv/GetFabricStatus.srv +++ b/capabilities2_msgs/srv/GetFabricStatus.srv @@ -4,4 +4,8 @@ std_msgs/Header header uint8 status uint8 FABRIC_IDLE=0 uint8 FABRIC_RUNNING=1 -uint8 FABRIC_FAILED=2 \ No newline at end of file +uint8 FABRIC_CANCELED=2 +uint8 FABRIC_ABORTED=3 +uint8 FABRIC_FAILED=4 +uint8 FABRIC_SUCCEEDED=5 +uint8 UNKNOWN=6 \ No newline at end of file From c534de9c8d5f6a778046f16fb418e1ee937facd7 Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Tue, 28 Jan 2025 02:14:45 +1100 Subject: [PATCH 079/133] Added composable launch to capabilities2 fabric --- capabilities2_fabric/CMakeLists.txt | 87 ++++++++++++++++++- .../capabilities_fabric.hpp | 2 +- ...ent.hpp => capabilities_fabric_client.hpp} | 4 +- .../launch/fabric_composed.launch.py | 49 +++++++++++ .../src/capabilities_fabric_client_comp.cpp | 4 + ...pp => capabilities_fabric_client_node.cpp} | 2 +- .../src/capabilities_fabric_comp.cpp | 4 + ...abric.cpp => capabilities_fabric_node.cpp} | 0 capabilities2_server/CMakeLists.txt | 10 ++- 9 files changed, 155 insertions(+), 7 deletions(-) rename capabilities2_fabric/include/capabilities2_fabric/{fabric_client.hpp => capabilities_fabric_client.hpp} (98%) create mode 100644 capabilities2_fabric/launch/fabric_composed.launch.py create mode 100644 capabilities2_fabric/src/capabilities_fabric_client_comp.cpp rename capabilities2_fabric/src/{fabric_client.cpp => capabilities_fabric_client_node.cpp} (81%) create mode 100644 capabilities2_fabric/src/capabilities_fabric_comp.cpp rename capabilities2_fabric/src/{capabilities_fabric.cpp => capabilities_fabric_node.cpp} (100%) diff --git a/capabilities2_fabric/CMakeLists.txt b/capabilities2_fabric/CMakeLists.txt index 3cd4cfb..71b7fbe 100644 --- a/capabilities2_fabric/CMakeLists.txt +++ b/capabilities2_fabric/CMakeLists.txt @@ -18,6 +18,7 @@ find_package(capabilities2_msgs REQUIRED) find_package(bondcpp REQUIRED) find_package(backward_ros REQUIRED) find_package(capabilities2_utils REQUIRED) +find_package(rclcpp_components REQUIRED) # Locate the static version of tinyxml2 find_library(TINYXML2_LIB NAMES tinyxml2 PATHS /usr/local/lib NO_DEFAULT_PATH) @@ -30,8 +31,12 @@ include_directories(include /usr/local/include ) +############################################################################ +# capabilities2_fabric node implementation that compiles as a executable +############################################################################ + add_executable(${PROJECT_NAME} - src/capabilities_fabric.cpp + src/capabilities_fabric_node.cpp ) target_link_libraries(${PROJECT_NAME} @@ -50,8 +55,47 @@ install(TARGETS ${PROJECT_NAME} DESTINATION lib/${PROJECT_NAME} ) +############################################################################ +# capabilities2_fabric component implementation that compiles as a library +############################################################################ + +add_library(${PROJECT_NAME}_comp SHARED + src/capabilities_fabric_comp.cpp +) + +target_link_libraries(${PROJECT_NAME}_comp + ${TINYXML2_LIB} +) + +ament_target_dependencies(${PROJECT_NAME}_comp + rclcpp + rclcpp_action + rclcpp_components + bondcpp + capabilities2_msgs + capabilities2_utils +) + +rclcpp_components_register_node(${PROJECT_NAME}_comp + PLUGIN "capabilities2_fabric::CapabilitiesFabric" + EXECUTABLE capabilities2_fabric_component +) + +ament_export_targets(capabilities2_fabric_component) + +install(TARGETS ${PROJECT_NAME}_comp + EXPORT capabilities2_fabric_component + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +############################################################################ +# fabric_client node implementation that compiles as a executable +############################################################################ + add_executable(fabric_client - src/fabric_client.cpp + src/capabilities_fabric_client_node.cpp ) target_link_libraries(fabric_client @@ -69,6 +113,45 @@ install(TARGETS fabric_client DESTINATION lib/${PROJECT_NAME} ) +############################################################################ +# fabric_client component implementation that compiles as a library +############################################################################ + +add_library(fabric_client_comp SHARED + src/capabilities_fabric_client_comp.cpp +) + +target_link_libraries(fabric_client_comp + ${TINYXML2_LIB} +) + +ament_target_dependencies(fabric_client_comp + rclcpp + rclcpp_action + rclcpp_components + bondcpp + capabilities2_msgs + capabilities2_utils +) + +rclcpp_components_register_node(fabric_client_comp + PLUGIN "capabilities2_fabric::CapabilitiesFabricClient" + EXECUTABLE capabilities2_fabric_client_component +) + +ament_export_targets(capabilities2_fabric_client_component) + +install(TARGETS fabric_client_comp + EXPORT capabilities2_fabric_client_component + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +############################################################################ +# miscellaneous +############################################################################ + install(DIRECTORY include/ DESTINATION include ) diff --git a/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric.hpp b/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric.hpp index 11978c3..824dfcc 100644 --- a/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric.hpp +++ b/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric.hpp @@ -55,7 +55,7 @@ class CapabilitiesFabric : public rclcpp::Node using ConfigureCapabilityClient = rclcpp::Client; using TriggerCapabilityClient = rclcpp::Client; - CapabilitiesFabric() : Node("Capabilities2_Fabric") + CapabilitiesFabric(const rclcpp::NodeOptions& options = rclcpp::NodeOptions()) : Node("Capabilities2_Fabric", options) { control_tag_list = xml_parser::get_control_list(); } diff --git a/capabilities2_fabric/include/capabilities2_fabric/fabric_client.hpp b/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric_client.hpp similarity index 98% rename from capabilities2_fabric/include/capabilities2_fabric/fabric_client.hpp rename to capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric_client.hpp index 46229b5..446725f 100644 --- a/capabilities2_fabric/include/capabilities2_fabric/fabric_client.hpp +++ b/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric_client.hpp @@ -46,7 +46,7 @@ class CapabilitiesFabricClient : public rclcpp::Node using SetFabricPlan = capabilities2_msgs::srv::SetFabricPlan; using CancelFabricPlan = capabilities2_msgs::srv::CancelFabricPlan; - CapabilitiesFabricClient() : Node("Capabilities2_Fabric_Client") + CapabilitiesFabricClient(const rclcpp::NodeOptions& options = rclcpp::NodeOptions()) : Node("Capabilities2_Fabric_Client", options) { declare_parameter("plan_file_path", "install/capabilities2_fabric/share/capabilities2_fabric/plans/default.xml"); plan_file_path = get_parameter("plan_file_path").as_string(); @@ -267,4 +267,4 @@ class CapabilitiesFabricClient : public rclcpp::Node /** Status of the fabric */ fabric_status fabric_state; -}; \ No newline at end of file +}; diff --git a/capabilities2_fabric/launch/fabric_composed.launch.py b/capabilities2_fabric/launch/fabric_composed.launch.py new file mode 100644 index 0000000..6215018 --- /dev/null +++ b/capabilities2_fabric/launch/fabric_composed.launch.py @@ -0,0 +1,49 @@ +''' +capabilities2_server launch file +''' + +import os +from launch import LaunchDescription +from launch_ros.actions import Node +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + """Generate launch description for capabilities2 server + + Returns: + LaunchDescription: The launch description for capabilities2 executor + """ + # load config file + fabric_config = os.path.join(get_package_share_directory('capabilities2_fabric'), 'config', 'fabric.yaml') + + # create bridge composition + fabric_container = ComposableNodeContainer( + name='capabilities2_fabric_container', + namespace='', + package='rclcpp_components', + executable='component_container_mt', + # prefix=['xterm -e gdb -ex run --args'], # Add GDB debugging prefix + arguments=['--ros-args', '--log-level', 'info'], + composable_node_descriptions=[ + ComposableNode( + package='capabilities2_fabric', + plugin='CapabilitiesFabric', + name='capabilities2_fabric', + output='screen' + ), + ComposableNode( + package='capabilities2_fabric', + plugin='CapabilitiesFabricClient', + name='fabric_client', + parameters=[fabric_config], + output='screen' + ) + ] + ) + + return LaunchDescription([ + fabric_container + ]) diff --git a/capabilities2_fabric/src/capabilities_fabric_client_comp.cpp b/capabilities2_fabric/src/capabilities_fabric_client_comp.cpp new file mode 100644 index 0000000..9189005 --- /dev/null +++ b/capabilities2_fabric/src/capabilities_fabric_client_comp.cpp @@ -0,0 +1,4 @@ +#include +#include + +RCLCPP_COMPONENTS_REGISTER_NODE(CapabilitiesFabricClient) diff --git a/capabilities2_fabric/src/fabric_client.cpp b/capabilities2_fabric/src/capabilities_fabric_client_node.cpp similarity index 81% rename from capabilities2_fabric/src/fabric_client.cpp rename to capabilities2_fabric/src/capabilities_fabric_client_node.cpp index 411a2f0..9bec80e 100644 --- a/capabilities2_fabric/src/fabric_client.cpp +++ b/capabilities2_fabric/src/capabilities_fabric_client_node.cpp @@ -1,4 +1,4 @@ -#include +#include int main(int argc, char* argv[]) { diff --git a/capabilities2_fabric/src/capabilities_fabric_comp.cpp b/capabilities2_fabric/src/capabilities_fabric_comp.cpp new file mode 100644 index 0000000..0219a1e --- /dev/null +++ b/capabilities2_fabric/src/capabilities_fabric_comp.cpp @@ -0,0 +1,4 @@ +#include +#include + +RCLCPP_COMPONENTS_REGISTER_NODE(CapabilitiesFabric) \ No newline at end of file diff --git a/capabilities2_fabric/src/capabilities_fabric.cpp b/capabilities2_fabric/src/capabilities_fabric_node.cpp similarity index 100% rename from capabilities2_fabric/src/capabilities_fabric.cpp rename to capabilities2_fabric/src/capabilities_fabric_node.cpp diff --git a/capabilities2_server/CMakeLists.txt b/capabilities2_server/CMakeLists.txt index d36df9b..d10f028 100644 --- a/capabilities2_server/CMakeLists.txt +++ b/capabilities2_server/CMakeLists.txt @@ -45,7 +45,9 @@ include_directories(include ${UUID_INCLUDE_DIRS} ) +####################################################################### # Component based implementation that compiles as a library +####################################################################### add_library(${PROJECT_NAME} SHARED src/capabilities_server_comp.cpp @@ -86,7 +88,9 @@ install(TARGETS ${PROJECT_NAME} RUNTIME DESTINATION bin ) -# Non Component based implementation that compiles as a executable +############################################################################ +# node implementation that compiles as a executable +############################################################################ add_executable(${PROJECT_NAME}_node src/capabilities_server_node.cpp @@ -129,6 +133,10 @@ target_link_libraries(test_capabilities_server ) add_dependencies(test_capabilities_server ${PROJECT_NAME}) +############################################################################ +# miscellaneous +############################################################################ + # install test executable install(TARGETS test_capabilities_server RUNTIME DESTINATION bin From 85f3791ecf239f2a69f6dfe5b84afd9cfa3c365d Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Wed, 29 Jan 2025 00:48:50 +1100 Subject: [PATCH 080/133] started improving the events publishing system of capabilities2 server --- capabilities2_fabric/CMakeLists.txt | 5 - .../capabilities_fabric.hpp | 20 +- .../capabilities_fabric_client.hpp | 4 +- .../utils}/bond_client.hpp | 0 .../utils}/connection.hpp | 0 .../utils}/status_client.hpp | 2 +- .../utils}/xml_parser.hpp | 4 +- capabilities2_msgs/msg/CapabilityEvent.msg | 30 ++- capabilities2_server/CMakeLists.txt | 3 - .../capabilities2_server/capabilities_api.hpp | 202 ++++++++++-------- .../capabilities2_server/capabilities_db.hpp | 3 +- .../capabilities_server.hpp | 88 ++++++-- .../capabilities2_server/models/header.hpp | 2 +- .../capabilities2_server/models/interface.hpp | 2 +- .../capabilities2_server/models/provider.hpp | 2 +- .../models/semantic_interface.hpp | 2 +- .../capabilities2_server/utils}/sql_safe.hpp | 0 capabilities2_utils/CMakeLists.txt | 24 --- capabilities2_utils/package.xml | 21 -- 19 files changed, 222 insertions(+), 192 deletions(-) rename {capabilities2_utils/include/capabilities2_utils => capabilities2_fabric/include/capabilities2_fabric/utils}/bond_client.hpp (100%) rename {capabilities2_utils/include/capabilities2_utils => capabilities2_fabric/include/capabilities2_fabric/utils}/connection.hpp (100%) rename {capabilities2_utils/include/capabilities2_utils => capabilities2_fabric/include/capabilities2_fabric/utils}/status_client.hpp (99%) rename {capabilities2_utils/include/capabilities2_utils => capabilities2_fabric/include/capabilities2_fabric/utils}/xml_parser.hpp (98%) rename {capabilities2_utils/include/capabilities2_utils => capabilities2_server/include/capabilities2_server/utils}/sql_safe.hpp (100%) delete mode 100644 capabilities2_utils/CMakeLists.txt delete mode 100644 capabilities2_utils/package.xml diff --git a/capabilities2_fabric/CMakeLists.txt b/capabilities2_fabric/CMakeLists.txt index 71b7fbe..3837283 100644 --- a/capabilities2_fabric/CMakeLists.txt +++ b/capabilities2_fabric/CMakeLists.txt @@ -17,7 +17,6 @@ find_package(rclcpp_action REQUIRED) find_package(capabilities2_msgs REQUIRED) find_package(bondcpp REQUIRED) find_package(backward_ros REQUIRED) -find_package(capabilities2_utils REQUIRED) find_package(rclcpp_components REQUIRED) # Locate the static version of tinyxml2 @@ -48,7 +47,6 @@ ament_target_dependencies(${PROJECT_NAME} rclcpp_action bondcpp capabilities2_msgs - capabilities2_utils ) install(TARGETS ${PROJECT_NAME} @@ -73,7 +71,6 @@ ament_target_dependencies(${PROJECT_NAME}_comp rclcpp_components bondcpp capabilities2_msgs - capabilities2_utils ) rclcpp_components_register_node(${PROJECT_NAME}_comp @@ -106,7 +103,6 @@ ament_target_dependencies(fabric_client rclcpp rclcpp_action capabilities2_msgs - capabilities2_utils ) install(TARGETS fabric_client @@ -131,7 +127,6 @@ ament_target_dependencies(fabric_client_comp rclcpp_components bondcpp capabilities2_msgs - capabilities2_utils ) rclcpp_components_register_node(fabric_client_comp diff --git a/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric.hpp b/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric.hpp index 824dfcc..e0b5f06 100644 --- a/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric.hpp +++ b/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric.hpp @@ -8,9 +8,9 @@ #include #include -#include -#include -#include +#include +#include +#include #include @@ -139,19 +139,7 @@ class CapabilitiesFabric : public rclcpp::Node status_->error("Received the request to cancel the plan"); (void)goal_handle; - if (connection_map.size() > 0) - { - // release all capabilities that were used since cancel request came - for (const auto& [key, value] : connection_map) - { - status = "Freeing capability of Node " + std::to_string(key) + " named " + value.source.runner; - process_feedback(status); - - free_capability(value.source.runner); - } - - bond_client_->stop(); - } + bond_client_->stop(); return rclcpp_action::CancelResponse::ACCEPT; } diff --git a/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric_client.hpp b/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric_client.hpp index 446725f..a13976c 100644 --- a/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric_client.hpp +++ b/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric_client.hpp @@ -10,8 +10,8 @@ #include #include -#include -#include +#include +#include #include diff --git a/capabilities2_utils/include/capabilities2_utils/bond_client.hpp b/capabilities2_fabric/include/capabilities2_fabric/utils/bond_client.hpp similarity index 100% rename from capabilities2_utils/include/capabilities2_utils/bond_client.hpp rename to capabilities2_fabric/include/capabilities2_fabric/utils/bond_client.hpp diff --git a/capabilities2_utils/include/capabilities2_utils/connection.hpp b/capabilities2_fabric/include/capabilities2_fabric/utils/connection.hpp similarity index 100% rename from capabilities2_utils/include/capabilities2_utils/connection.hpp rename to capabilities2_fabric/include/capabilities2_fabric/utils/connection.hpp diff --git a/capabilities2_utils/include/capabilities2_utils/status_client.hpp b/capabilities2_fabric/include/capabilities2_fabric/utils/status_client.hpp similarity index 99% rename from capabilities2_utils/include/capabilities2_utils/status_client.hpp rename to capabilities2_fabric/include/capabilities2_fabric/utils/status_client.hpp index c9afd96..c634155 100644 --- a/capabilities2_utils/include/capabilities2_utils/status_client.hpp +++ b/capabilities2_fabric/include/capabilities2_fabric/utils/status_client.hpp @@ -21,7 +21,7 @@ class StatusClient status_publisher_ = node_->create_publisher(topic_name, 10); } - /** + /**error_element * @brief publishes status information to the given topic and prints to the logging as info * * @param text Text to be published diff --git a/capabilities2_utils/include/capabilities2_utils/xml_parser.hpp b/capabilities2_fabric/include/capabilities2_fabric/utils/xml_parser.hpp similarity index 98% rename from capabilities2_utils/include/capabilities2_utils/xml_parser.hpp rename to capabilities2_fabric/include/capabilities2_fabric/utils/xml_parser.hpp index 991b5eb..71f450d 100644 --- a/capabilities2_utils/include/capabilities2_utils/xml_parser.hpp +++ b/capabilities2_fabric/include/capabilities2_fabric/utils/xml_parser.hpp @@ -3,8 +3,8 @@ #include #include #include -#include -#include +#include +#include namespace xml_parser { diff --git a/capabilities2_msgs/msg/CapabilityEvent.msg b/capabilities2_msgs/msg/CapabilityEvent.msg index 1a52c94..1062519 100644 --- a/capabilities2_msgs/msg/CapabilityEvent.msg +++ b/capabilities2_msgs/msg/CapabilityEvent.msg @@ -1,16 +1,30 @@ std_msgs/Header header + # Capability which this event pertains to -string capability -# Capability provider which this event pertains to -string provider +Capability source + +# Capability which this event targets at +Capability target + +# Capability trigger thread id +int8 thread_id + +# status message +string text # Event types -string LAUNCHED="launched" -string STOPPED="stopped" -string TERMINATED="terminated" -string SERVER_READY="server_ready" +uint8 IDLE=0 +uint8 STARTED=1 +uint8 STOPPED=2 +uint8 TERMINATED=3 +uint8 SUCCEEDED=4 +uint8 UNDEFINED=5 + # Event type -string type +uint8 event + +# Whether the server configuration is complete +bool server_ready # PID of the related process int32 pid diff --git a/capabilities2_server/CMakeLists.txt b/capabilities2_server/CMakeLists.txt index d10f028..aa21c61 100644 --- a/capabilities2_server/CMakeLists.txt +++ b/capabilities2_server/CMakeLists.txt @@ -19,7 +19,6 @@ find_package(pluginlib REQUIRED) find_package(rclcpp_components REQUIRED) find_package(capabilities2_msgs REQUIRED) find_package(capabilities2_runner REQUIRED) -find_package(capabilities2_utils REQUIRED) find_package(backward_ros REQUIRED) find_package(PkgConfig) @@ -71,7 +70,6 @@ ament_target_dependencies(${PROJECT_NAME} SQLite3 yaml-cpp UUID - capabilities2_utils ) rclcpp_components_register_node(${PROJECT_NAME} @@ -112,7 +110,6 @@ ament_target_dependencies(${PROJECT_NAME}_node capabilities2_msgs capabilities2_runner SQLite3 - capabilities2_utils yaml-cpp UUID ) diff --git a/capabilities2_server/include/capabilities2_server/capabilities_api.hpp b/capabilities2_server/include/capabilities2_server/capabilities_api.hpp index 55acc3b..3d19640 100644 --- a/capabilities2_server/include/capabilities2_server/capabilities_api.hpp +++ b/capabilities2_server/include/capabilities2_server/capabilities_api.hpp @@ -26,6 +26,15 @@ namespace capabilities2_server { +enum capability_event +{ + IDLE, + STARTED, + STOPPED, + TERMINATED, + SUCCEEDED +}; + /** * @brief capabilities api * mapping message based logic of capabilities to database models @@ -613,105 +622,116 @@ class CapabilitiesAPI return running_capabilities; } - // event api - // related to runner api - const capabilities2_msgs::msg::CapabilityEvent on_capability_started(const std::string& capability) + /** + * @brief get pid of capability + * + * @param capability capability of which the pid is requested + * @return value of pid + */ + int get_pid(const std::string& capability) { - // create event msg - capabilities2_msgs::msg::CapabilityEvent event; - event.header.frame_id = "capabilities"; - event.header.stamp = rclcpp::Clock().now(); - - // started event - event.type = capabilities2_msgs::msg::CapabilityEvent::LAUNCHED; - - // set cap, prov, pid - event.capability = capability; - - // try to get details - try - { - event.provider = runner_cache_.provider(capability); - event.pid = atoi(runner_cache_.pid(capability).c_str()); - } - catch (const capabilities2_runner::runner_exception& e) - { - event.provider = "unknown"; - event.pid = -1; - } - - return event; + return atoi(runner_cache_.pid(capability).c_str()); } - const capabilities2_msgs::msg::CapabilityEvent on_capability_stopped(const std::string& capability) - { - // create event msg - capabilities2_msgs::msg::CapabilityEvent event; - event.header.frame_id = "capabilities"; - event.header.stamp = rclcpp::Clock().now(); - - // terminated event - event.type = capabilities2_msgs::msg::CapabilityEvent::STOPPED; - - // set cap, prov, pid - event.capability = capability; - - // try to get details - try - { - event.provider = runner_cache_.provider(capability); - event.pid = atoi(runner_cache_.pid(capability).c_str()); - } - catch (const capabilities2_runner::runner_exception& e) - { - event.provider = "unknown"; - event.pid = -1; - } - - return event; - } - - const capabilities2_msgs::msg::CapabilityEvent on_capability_terminated(const std::string& capability) - { - // create event msg - capabilities2_msgs::msg::CapabilityEvent event; - event.header.frame_id = "capabilities"; - event.header.stamp = rclcpp::Clock().now(); - - // terminated event - event.type = capabilities2_msgs::msg::CapabilityEvent::TERMINATED; - - // set cap, prov, pid - event.capability = capability; - try - { - event.provider = runner_cache_.provider(capability); - event.pid = atoi(runner_cache_.pid(capability).c_str()); - } - catch (const capabilities2_runner::runner_exception& e) - { - event.provider = "unknown"; - event.pid = -1; - } + // event api + // related to runner api + // const capabilities2_msgs::msg::CapabilityEvent on_capability_started(const std::string& capability) + // { + // // create event msg + // capabilities2_msgs::msg::CapabilityEvent event; + // event.header.frame_id = "capabilities"; + // event.header.stamp = rclcpp::Clock().now(); + + // // started event + // event.type = capabilities2_msgs::msg::CapabilityEvent::LAUNCHED; + + // // set cap, prov, pid + // event.capability = capability; + + // // try to get details + // try + // { + // event.provider = runner_cache_.provider(capability); + // event.pid = atoi(runner_cache_.pid(capability).c_str()); + // } + // catch (const capabilities2_runner::runner_exception& e) + // { + // event.provider = "unknown"; + // event.pid = -1; + // } + + // return event; + // } - // log error - RCLCPP_ERROR(node_logging_interface_ptr_->get_logger(), "capability terminated: %s", capability.c_str()); + // const capabilities2_msgs::msg::CapabilityEvent on_capability_stopped(const std::string& capability) + // { + // // create event msg + // capabilities2_msgs::msg::CapabilityEvent event; + // event.header.frame_id = "capabilities"; + // event.header.stamp = rclcpp::Clock().now(); + + // // terminated event + // event.type = capabilities2_msgs::msg::CapabilityEvent::STOPPED; + + // // set cap, prov, pid + // event.capability = capability; + + // // try to get details + // try + // { + // event.provider = runner_cache_.provider(capability); + // event.pid = atoi(runner_cache_.pid(capability).c_str()); + // } + // catch (const capabilities2_runner::runner_exception& e) + // { + // event.provider = "unknown"; + // event.pid = -1; + // } + + // return event; + // } - return event; - } + // const capabilities2_msgs::msg::CapabilityEvent on_capability_terminated(const std::string& capability) + // { + // // create event msg + // capabilities2_msgs::msg::CapabilityEvent event; + // event.header.frame_id = "capabilities"; + // event.header.stamp = rclcpp::Clock().now(); + + // // terminated event + // event.type = capabilities2_msgs::msg::CapabilityEvent::TERMINATED; + + // // set cap, prov, pid + // event.capability = capability; + // try + // { + // event.provider = runner_cache_.provider(capability); + // event.pid = atoi(runner_cache_.pid(capability).c_str()); + // } + // catch (const capabilities2_runner::runner_exception& e) + // { + // event.provider = "unknown"; + // event.pid = -1; + // } + + // // log error + // RCLCPP_ERROR(node_logging_interface_ptr_->get_logger(), "capability terminated: %s", capability.c_str()); + + // return event; + // } - const capabilities2_msgs::msg::CapabilityEvent on_server_ready() - { - // create event msg - capabilities2_msgs::msg::CapabilityEvent event; - event.header.frame_id = "capabilities"; - event.header.stamp = rclcpp::Clock().now(); + // const capabilities2_msgs::msg::CapabilityEvent on_server_ready() + // { + // // create event msg + // capabilities2_msgs::msg::CapabilityEvent event; + // event.header.frame_id = "capabilities"; + // event.header.stamp = rclcpp::Clock().now(); - // started event - event.type = capabilities2_msgs::msg::CapabilityEvent::SERVER_READY; + // // started event + // event.type = capabilities2_msgs::msg::CapabilityEvent::SERVER_READY; - return event; - } + // return event; + // } // bond api // establish bond diff --git a/capabilities2_server/include/capabilities2_server/capabilities_db.hpp b/capabilities2_server/include/capabilities2_server/capabilities_db.hpp index e5ff407..d48e7ec 100644 --- a/capabilities2_server/include/capabilities2_server/capabilities_db.hpp +++ b/capabilities2_server/include/capabilities2_server/capabilities_db.hpp @@ -13,8 +13,7 @@ #include #include #include - -#include +#include namespace capabilities2_server { diff --git a/capabilities2_server/include/capabilities2_server/capabilities_server.hpp b/capabilities2_server/include/capabilities2_server/capabilities_server.hpp index 517c065..6e6b2b5 100644 --- a/capabilities2_server/include/capabilities2_server/capabilities_server.hpp +++ b/capabilities2_server/include/capabilities2_server/capabilities_server.hpp @@ -62,6 +62,9 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI CapabilitiesServer(const rclcpp::NodeOptions& options = rclcpp::NodeOptions()) : Node("capabilities2", options), CapabilitiesAPI() { + // pubs + event_pub_ = create_publisher("~/events", 10); + // params interface // loop rate declare_parameter("loop_rate", 5.0); @@ -86,7 +89,8 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI if (rebuild) { // remove db file if it exists - RCLCPP_INFO(get_logger(), "Rebuilding capabilities database"); + event_publish("Rebuilding capabilities database", false); + if (std::remove(db_file.c_str()) != 0) { RCLCPP_ERROR(get_logger(), "Error deleting file %s", db_file.c_str()); @@ -97,12 +101,17 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI if (!std::filesystem::exists(db_path)) { // create db file path + event_publish("Creating capabilities database", false); + std::filesystem::create_directories(db_path.parent_path()); } // init capabilities api + event_publish("Connecting API with Database", false); + connect(db_file, get_node_logging_interface()); - + + event_publish("Loading capabilities", false); // load capabilities from package paths for (const auto& package_path : package_paths) @@ -110,11 +119,7 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI load_capabilities(package_path); } - // pubs - // events - event_pub_ = create_publisher("~/events", 10); - - // subs + event_publish("Starting server interfaces", false); // services // establish bond @@ -186,11 +191,8 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI "~/get_running_capabilities", std::bind(&CapabilitiesServer::get_running_capabilities_cb, this, std::placeholders::_1, std::placeholders::_2)); - // log ready - RCLCPP_INFO(get_logger(), "capabilities server started"); - // publish ready event - event_pub_->publish(on_server_ready()); + event_publish("capabilities server start up complete"); } // service callbacks @@ -495,8 +497,11 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI { // read spec file load_spec_content(package_path + "/" + package + "/" + spec_path, capability_spec); + // add capability to db - RCLCPP_INFO(get_logger(), "adding capability: %s", (package + "/" + spec_path).c_str()); + std::string status = "adding capability: " + package + "/" + spec_path; + event_publish(status); + add_capability(capability_spec); } catch (const std::runtime_error& e) @@ -576,8 +581,11 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI { // read spec file load_spec_content(package_path + "/" + package + "/share/" + package + "/" + spec_path, capability_spec); + // add capability to db - RCLCPP_INFO(get_logger(), "adding capability: %s", (package + "/" + spec_path).c_str()); + std::string status = "adding capability: " + package + "/" + spec_path; + event_publish(status); + add_capability(capability_spec); } catch (const std::runtime_error& e) @@ -648,6 +656,60 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI spec_file_file.close(); } + void event_publish(const std::string& text, bool is_server_ready = true) + { + auto message = capabilities2_msgs::msg::CapabilityEvent(); + + message.header.stamp = rclcpp::Clock().now(); + message.source.capability = ""; + message.source.provider = ""; + message.target.capability = ""; + message.target.provider = ""; + message.thread_id = 0; + message.text = text; + message.pid = -1; + message.server_ready = is_server_ready; + + event_pub_->publish(message); + + RCLCPP_INFO(get_logger(), text.c_str()); + } + + void event_publish_runner(const std::string& capability, const std::string& provider, int thread_id, + const std::string& text, const std::string target_capability = "", + const std::string target_provider = "", + enum capability_event event = capability_event::IDLE) + { + auto message = capabilities2_msgs::msg::CapabilityEvent(); + + message.header.stamp = rclcpp::Clock().now(); + message.source.capability = capability; + message.source.provider = provider; + message.target.capability = target_capability; + message.target.provider = target_provider; + message.thread_id = thread_id; + message.text = text; + message.pid = get_pid(capability); + message.server_ready = true; + + if (event == capability_event::IDLE) + message.event = capabilities2_msgs::msg::CapabilityEvent::IDLE; + else if (event == capability_event::STARTED) + message.event = capabilities2_msgs::msg::CapabilityEvent::STARTED; + else if (event == capability_event::STOPPED) + message.event = capabilities2_msgs::msg::CapabilityEvent::STOPPED; + else if (event == capability_event::TERMINATED) + message.event = capabilities2_msgs::msg::CapabilityEvent::TERMINATED; + else if (event == capability_event::SUCCEEDED) + message.event = capabilities2_msgs::msg::CapabilityEvent::SUCCEEDED; + else + message.event = capabilities2_msgs::msg::CapabilityEvent::UNDEFINED; + + event_pub_->publish(message); + + RCLCPP_INFO(get_logger(), text.c_str()); + } + private: // loop hz double loop_hz_; diff --git a/capabilities2_server/include/capabilities2_server/models/header.hpp b/capabilities2_server/include/capabilities2_server/models/header.hpp index 0faa212..7dc9738 100644 --- a/capabilities2_server/include/capabilities2_server/models/header.hpp +++ b/capabilities2_server/include/capabilities2_server/models/header.hpp @@ -2,7 +2,7 @@ #include #include -#include +#include namespace capabilities2_server { diff --git a/capabilities2_server/include/capabilities2_server/models/interface.hpp b/capabilities2_server/include/capabilities2_server/models/interface.hpp index 0976a31..f6a47b0 100644 --- a/capabilities2_server/include/capabilities2_server/models/interface.hpp +++ b/capabilities2_server/include/capabilities2_server/models/interface.hpp @@ -5,7 +5,7 @@ #include #include #include -#include +#include namespace capabilities2_server { diff --git a/capabilities2_server/include/capabilities2_server/models/provider.hpp b/capabilities2_server/include/capabilities2_server/models/provider.hpp index e436493..fa1088b 100644 --- a/capabilities2_server/include/capabilities2_server/models/provider.hpp +++ b/capabilities2_server/include/capabilities2_server/models/provider.hpp @@ -6,7 +6,7 @@ #include #include #include -#include +#include namespace capabilities2_server { diff --git a/capabilities2_server/include/capabilities2_server/models/semantic_interface.hpp b/capabilities2_server/include/capabilities2_server/models/semantic_interface.hpp index 864a564..0a727bd 100644 --- a/capabilities2_server/include/capabilities2_server/models/semantic_interface.hpp +++ b/capabilities2_server/include/capabilities2_server/models/semantic_interface.hpp @@ -4,7 +4,7 @@ #include #include #include -#include +#include namespace capabilities2_server { diff --git a/capabilities2_utils/include/capabilities2_utils/sql_safe.hpp b/capabilities2_server/include/capabilities2_server/utils/sql_safe.hpp similarity index 100% rename from capabilities2_utils/include/capabilities2_utils/sql_safe.hpp rename to capabilities2_server/include/capabilities2_server/utils/sql_safe.hpp diff --git a/capabilities2_utils/CMakeLists.txt b/capabilities2_utils/CMakeLists.txt deleted file mode 100644 index 6c80272..0000000 --- a/capabilities2_utils/CMakeLists.txt +++ /dev/null @@ -1,24 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(capabilities2_utils) - -# Default to C++17 -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) -endif() - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -find_package(ament_cmake REQUIRED) -find_package(rclcpp REQUIRED) -find_package(bondcpp REQUIRED) - -install(DIRECTORY include/ - DESTINATION include -) - -ament_export_include_directories(include) -ament_export_libraries(${PROJECT_NAME}) - -ament_package() diff --git a/capabilities2_utils/package.xml b/capabilities2_utils/package.xml deleted file mode 100644 index bd910d3..0000000 --- a/capabilities2_utils/package.xml +++ /dev/null @@ -1,21 +0,0 @@ - - - - capabilities2_utils - 0.0.0 - TODO: Package description - kalana - TODO: License declaration - - ament_cmake - - ament_lint_auto - ament_lint_common - - rclcpp - bondcpp - - - ament_cmake - - From 0fed03ebe9c57226713d3f3e9c4f61d94da1c75d Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Mon, 3 Feb 2025 15:43:41 +1100 Subject: [PATCH 081/133] improved event publishing structure of capabilities2 server --- capabilities2_msgs/msg/CapabilityEvent.msg | 5 +- .../capabilities2_runner/action_runner.hpp | 59 ++--- .../capabilities2_runner/encap_runner.hpp | 4 +- .../capabilities2_runner/launch_runner.hpp | 17 +- .../capabilities2_runner/runner_base.hpp | 82 ++++++- .../capabilities2_runner/service_runner.hpp | 37 +++- .../capabilities2_runner/topic_runner.hpp | 10 +- .../utils/capability_event_t.hpp | 13 ++ capabilities2_runner/src/standard_runners.cpp | 5 +- .../listen_runner.hpp | 7 +- .../speak_runner.hpp | 15 +- .../src/audio_runners.cpp | 5 - .../bt_factory_runner.hpp | 5 +- capabilities2_runner_bt/src/bt_runners.cpp | 4 - .../occupancygrid_runner.hpp | 11 +- .../robotpose_runner.hpp | 5 +- .../waypoint_runner.hpp | 11 +- .../src/nav2_runners.cpp | 5 - .../prompt_occupancy_runner.hpp | 11 +- .../prompt_plan_request_runner.hpp | 7 +- .../prompt_plan_response_runner.hpp | 7 +- .../prompt_pose_runner.hpp | 10 +- .../prompt_text_runner.hpp | 7 +- .../src/prompt_runners.cpp | 5 - .../capabilities2_server/capabilities_api.hpp | 208 ++++-------------- .../capabilities_server.hpp | 100 ++++----- .../capabilities2_server/runner_cache.hpp | 36 ++- 27 files changed, 340 insertions(+), 351 deletions(-) create mode 100644 capabilities2_runner/include/capabilities2_runner/utils/capability_event_t.hpp diff --git a/capabilities2_msgs/msg/CapabilityEvent.msg b/capabilities2_msgs/msg/CapabilityEvent.msg index 1062519..581a67d 100644 --- a/capabilities2_msgs/msg/CapabilityEvent.msg +++ b/capabilities2_msgs/msg/CapabilityEvent.msg @@ -16,7 +16,7 @@ string text uint8 IDLE=0 uint8 STARTED=1 uint8 STOPPED=2 -uint8 TERMINATED=3 +uint8 FAILED=3 uint8 SUCCEEDED=4 uint8 UNDEFINED=5 @@ -26,5 +26,8 @@ uint8 event # Whether the server configuration is complete bool server_ready +# Whether a failure occured +bool error + # PID of the related process int32 pid diff --git a/capabilities2_runner/include/capabilities2_runner/action_runner.hpp b/capabilities2_runner/include/capabilities2_runner/action_runner.hpp index bcef201..4ca876e 100644 --- a/capabilities2_runner/include/capabilities2_runner/action_runner.hpp +++ b/capabilities2_runner/include/capabilities2_runner/action_runner.hpp @@ -39,25 +39,25 @@ class ActionRunner : public RunnerBase * @param run_config runner configuration loaded from the yaml file * @param action_name action name used in the yaml file, used to load specific configuration from the run_config */ - virtual void init_action(rclcpp::Node::SharedPtr node, const runner_opts& run_config, const std::string& action_name) + virtual void init_action(rclcpp::Node::SharedPtr node, const runner_opts& run_config, const std::string& action_name, + std::function runner_publish_func) { // initialize the runner base by storing node pointer and run config - init_base(node, run_config); + init_base(node, run_config, runner_publish_func); // 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()); + info_("waiting for action: " + action_name); if (!action_client_->wait_for_action_server(std::chrono::seconds(1000))) { - RCLCPP_ERROR(node_->get_logger(), "[%s] failed to connect to action: %s", run_config_.interface.c_str(), - action_name.c_str()); + error_("failed to connect to action: " + action_name); throw runner_exception("failed to connect to action server"); } - RCLCPP_INFO(node_->get_logger(), "[%s] connected with action: %s", run_config_.interface.c_str(), - action_name.c_str()); + + info_("connected with action: " + action_name); } /** @@ -88,14 +88,13 @@ class ActionRunner : public RunnerBase if (response->return_code != action_msgs::srv::CancelGoal_Response::ERROR_NONE) { // throw runner_exception("failed to cancel runner"); - RCLCPP_WARN(node_->get_logger(), "Runner cancellation failed."); + error_("Runner cancellation failed."); } // Trigger on_stopped event if defined if (events[execute_id].on_stopped != "") { - RCLCPP_INFO(node_->get_logger(), "[%s] on_stopped event available. Triggering", - run_config_.interface.c_str()); + info_("on_stopped", -1, events[execute_id].on_stopped, EventType::STOPPED); triggerFunction_(events[execute_id].on_stopped, update_on_stopped(events[execute_id].on_stopped_param)); } }); @@ -112,7 +111,7 @@ class ActionRunner : public RunnerBase } catch (const rclcpp_action::exceptions::UnknownGoalHandleError& e) { - RCLCPP_ERROR(node_->get_logger(), "failed to cancel goal: %s", e.what()); + error_("failed to cancel goal: " + std::string(e.what())); throw runner_exception(e.what()); } } @@ -134,9 +133,9 @@ class ActionRunner : public RunnerBase throw runner_exception("cannot trigger action without parameters"); // generate a goal from parameters if provided - goal_msg_ = generate_goal(parameters_[id]); + goal_msg_ = generate_goal(parameters_[id], id); - RCLCPP_INFO(node_->get_logger(), "[%s/%d] goal generated.", run_config_.interface.c_str(), id); + info_("goal generated", id); std::unique_lock lock(send_goal_mutex); action_complete = false; @@ -146,21 +145,18 @@ class ActionRunner : public RunnerBase [this, id](const typename rclcpp_action::ClientGoalHandle::SharedPtr& goal_handle) { if (goal_handle) { - RCLCPP_INFO(node_->get_logger(), "[%s/%d] goal accepted. Waiting for result", run_config_.interface.c_str(), - id); + info_("goal accepted. Waiting for result", id); // trigger the events related to on_started state if (events[execute_id].on_started != "") { - RCLCPP_INFO(node_->get_logger(), "[%s/%d] on_started event available. Triggering", - run_config_.interface.c_str(), id); - + info_("on_started", id, events[execute_id].on_started, EventType::STARTED); triggerFunction_(events[execute_id].on_started, update_on_started(events[execute_id].on_started_param)); } } else { - RCLCPP_ERROR(node_->get_logger(), "[%s/%d] goal rejected", run_config_.interface.c_str(), id); + error_("goal rejected", id); } // store goal handle to be used with stop funtion @@ -174,38 +170,32 @@ class ActionRunner : public RunnerBase if (feedback != "") { - RCLCPP_INFO(node_->get_logger(), "[%s/%d] received feedback: %s", run_config_.interface.c_str(), id, - feedback.c_str()); + info_("received feedback: " + feedback, id); } }; send_goal_options_.result_callback = [this, id](const typename rclcpp_action::ClientGoalHandle::WrappedResult& wrapped_result) { - RCLCPP_INFO(node_->get_logger(), "[%s/%d] received result", run_config_.interface.c_str(), id); - + info_("received result", id); if (wrapped_result.code == rclcpp_action::ResultCode::SUCCEEDED) { - RCLCPP_INFO(node_->get_logger(), "[%s/%d] action succeeded.", run_config_.interface.c_str(), id); + info_("action succeeded.", id); // trigger the events related to on_success state if (events[execute_id].on_success != "") { - RCLCPP_INFO(node_->get_logger(), "[%s/%d] on_success event available. Triggering", - run_config_.interface.c_str(), id); + info_("on_success", id, events[execute_id].on_success, EventType::SUCCEEDED); triggerFunction_(events[execute_id].on_success, update_on_success(events[execute_id].on_success_param)); } } else { - RCLCPP_ERROR(node_->get_logger(), "[%s/%d] action failed.", run_config_.interface.c_str(), id); - RCLCPP_ERROR(node_->get_logger(), "[%s/%d] error code: %d", run_config_.interface.c_str(), id, - static_cast(wrapped_result.code)); + error_("action failed", id); // trigger the events related to on_failure state if (events[execute_id].on_failure != "") { - RCLCPP_INFO(node_->get_logger(), "[%s/%d] on_failure event available. Triggering", - run_config_.interface.c_str(), id); + info_("on_failure", id, events[execute_id].on_failure, EventType::FAILED); triggerFunction_(events[execute_id].on_failure, update_on_failure(events[execute_id].on_failure_param)); } } @@ -216,12 +206,11 @@ class ActionRunner : public RunnerBase }; goal_handle_future_ = action_client_->async_send_goal(goal_msg_, send_goal_options_); - - RCLCPP_INFO(node_->get_logger(), "[%s/%d] goal sent. Waiting for acceptance", run_config_.interface.c_str(), id); + info_("goal sent. Waiting for acceptance.", id); // Conditional wait send_goal_cv.wait(lock, [this] { return action_complete; }); - RCLCPP_INFO(node_->get_logger(), "[%s/%d] action complete. Thread closing.", run_config_.interface.c_str(), id); + info_("action complete. Thread closing.", id); } protected: @@ -236,7 +225,7 @@ class ActionRunner : public RunnerBase * @param parameters * @return ActionT::Goal the generated goal */ - virtual typename ActionT::Goal generate_goal(tinyxml2::XMLElement* parameters) = 0; + virtual typename ActionT::Goal generate_goal(tinyxml2::XMLElement* parameters, int id) = 0; /** * @brief Generate a std::string from feedback message diff --git a/capabilities2_runner/include/capabilities2_runner/encap_runner.hpp b/capabilities2_runner/include/capabilities2_runner/encap_runner.hpp index 084bb64..931454e 100644 --- a/capabilities2_runner/include/capabilities2_runner/encap_runner.hpp +++ b/capabilities2_runner/include/capabilities2_runner/encap_runner.hpp @@ -38,10 +38,10 @@ class EnCapRunner : public NoTriggerActionRunner * @param action_name action name used in the yaml file, used to load specific configuration from the run_config */ virtual void init_encapsulated_action(rclcpp::Node::SharedPtr node, const runner_opts& run_config, - const std::string& action_name) + const std::string& action_name, std::function print) { // init the base action runner - init_action(node, run_config, action_name); + init_action(node, run_config, action_name, print); // create an encapsulating action server encap_action_ = rclcpp_action::create_server( diff --git a/capabilities2_runner/include/capabilities2_runner/launch_runner.hpp b/capabilities2_runner/include/capabilities2_runner/launch_runner.hpp index 2697bf3..f2f6ea5 100644 --- a/capabilities2_runner/include/capabilities2_runner/launch_runner.hpp +++ b/capabilities2_runner/include/capabilities2_runner/launch_runner.hpp @@ -29,9 +29,10 @@ class LaunchRunner : public RunnerBase * @param node shared pointer to the capabilities node. Allows to use ros node related functionalities * @param run_config runner configuration loaded from the yaml file */ - virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config) override + virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config, + std::function print) override { - init_base(node, run_config); + init_base(node, run_config, print); package_name = run_config_.runner.substr(0, run_config_.runner.find("/")); launch_name = run_config_.runner.substr(run_config_.runner.find("/") + 1); @@ -81,11 +82,13 @@ class LaunchRunner : public RunnerBase request_msg, [this](typename rclcpp::Client::SharedFuture future) { if (!future.valid()) { - RCLCPP_ERROR(node_->get_logger(), "Request to launch %s from %s failed", launch_name.c_str(), package_name.c_str()); + RCLCPP_ERROR(node_->get_logger(), "Request to launch %s from %s failed", launch_name.c_str(), + package_name.c_str()); return; } - RCLCPP_INFO(node_->get_logger(), "Request to launch %s from %s succeeded", launch_name.c_str(), package_name.c_str()); + RCLCPP_INFO(node_->get_logger(), "Request to launch %s from %s succeeded", launch_name.c_str(), + package_name.c_str()); }); } @@ -113,11 +116,13 @@ class LaunchRunner : public RunnerBase request_msg, [this](typename rclcpp::Client::SharedFuture future) { if (!future.valid()) { - RCLCPP_ERROR(node_->get_logger(), "Request to stop %s from %s failed ", launch_name.c_str(), package_name.c_str()); + RCLCPP_ERROR(node_->get_logger(), "Request to stop %s from %s failed ", launch_name.c_str(), + package_name.c_str()); return; } - RCLCPP_INFO(node_->get_logger(), "Request to launch %s from %s succeeded ", launch_name.c_str(), package_name.c_str()); + RCLCPP_INFO(node_->get_logger(), "Request to launch %s from %s succeeded ", launch_name.c_str(), + package_name.c_str()); }); } diff --git a/capabilities2_runner/include/capabilities2_runner/runner_base.hpp b/capabilities2_runner/include/capabilities2_runner/runner_base.hpp index 2733b44..46249b3 100644 --- a/capabilities2_runner/include/capabilities2_runner/runner_base.hpp +++ b/capabilities2_runner/include/capabilities2_runner/runner_base.hpp @@ -7,6 +7,8 @@ #include #include #include +#include +#include namespace capabilities2_runner { @@ -88,6 +90,9 @@ struct event_opts class RunnerBase { public: + using Event = capabilities2_msgs::msg::CapabilityEvent; + using EventType = capabilities2_runner::capability_event; + RunnerBase() : run_config_() { } @@ -103,8 +108,10 @@ class RunnerBase * * @param node shared pointer to the capabilities node. Allows to use ros node related functionalities * @param run_config runner configuration loaded from the yaml file + * @param print_ */ - virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config) = 0; + virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config, + std::function print) = 0; /** * @brief stop the runner @@ -123,13 +130,13 @@ class RunnerBase */ virtual void trigger(const std::string& parameters) { - RCLCPP_INFO(node_->get_logger(), "[%s/%d] received new parameters", run_config_.interface.c_str(), thread_id); + info_("received new parameters", thread_id); parameters_[thread_id] = convert_to_xml(parameters); executionThreadPool[thread_id] = std::thread(&RunnerBase::execution, this, thread_id); - RCLCPP_INFO(node_->get_logger(), "[%s/%d] started execution", run_config_.interface.c_str(), thread_id); + info_("started execution", thread_id); thread_id += 1; } @@ -140,11 +147,14 @@ class RunnerBase * @param node shared pointer to the capabilities node. Allows to use ros node related functionalities * @param run_config runner configuration loaded from the yaml file */ - void init_base(rclcpp::Node::SharedPtr node, const runner_opts& run_config) + void init_base(rclcpp::Node::SharedPtr node, const runner_opts& run_config, + std::function print) { // store node pointer and opts node_ = node; run_config_ = run_config; + print_ = print; + insert_id = 0; execute_id = -1; thread_id = 0; @@ -160,8 +170,7 @@ class RunnerBase int attach_events(capabilities2_runner::event_opts& event_option, std::function triggerFunction) { - RCLCPP_INFO(node_->get_logger(), "[%s] accepted event options with ID : %d ", run_config_.interface.c_str(), - insert_id); + info_("accepted event options with ID : " + std::to_string(insert_id)); triggerFunction_ = triggerFunction; @@ -464,6 +473,62 @@ class RunnerBase } protected: + void info_(const std::string text, int thread_id = -1, const std::string& tcapability = "", + EventType event = EventType::IDLE) + { + auto message = Event(); + + message.header.stamp = rclcpp::Clock().now(); + message.source.capability = run_config_.interface; + message.source.provider = run_config_.provider; + message.target.capability = tcapability; + message.thread_id = thread_id; + message.text = text; + message.error = false; + message.server_ready = true; + + switch (event) + { + case EventType::IDLE: + message.event = Event::IDLE; + break; + case EventType::STARTED: + message.event = Event::STARTED; + break; + case EventType::STOPPED: + message.event = Event::STOPPED; + break; + case EventType::FAILED: + message.event = Event::FAILED; + break; + case EventType::SUCCEEDED: + message.event = Event::SUCCEEDED; + break; + default: + message.event = Event::UNDEFINED; + break; + } + + print_(message); + } + + void error_(const std::string text, int thread_id = -1) + { + auto message = Event(); + + message.header.stamp = rclcpp::Clock().now(); + message.source.capability = run_config_.interface; + message.source.provider = run_config_.provider; + message.target.capability = ""; + message.thread_id = thread_id; + message.text = text; + message.error = true; + message.server_ready = true; + message.event = Event::IDLE; + + print_(message); + } + /** * @brief shared pointer to the capabilities node. Allows to use ros node related functionalities */ @@ -520,6 +585,11 @@ class RunnerBase */ std::function triggerFunction_; + /** + * @brief event function for internal runner event publishing + */ + std::function print_; + /** * @brief XMLElement that is used to convert xml strings to std::string */ diff --git a/capabilities2_runner/include/capabilities2_runner/service_runner.hpp b/capabilities2_runner/include/capabilities2_runner/service_runner.hpp index 6fbf2d2..46040a8 100644 --- a/capabilities2_runner/include/capabilities2_runner/service_runner.hpp +++ b/capabilities2_runner/include/capabilities2_runner/service_runner.hpp @@ -32,23 +32,24 @@ class ServiceRunner : public RunnerBase * @param service_name action name used in the yaml file, used to load specific configuration from the run_config */ virtual void init_service(rclcpp::Node::SharedPtr node, const runner_opts& run_config, - const std::string& service_name) + const std::string& service_name, std::function print) { // initialize the runner base by storing node pointer and run config - init_base(node, run_config); + init_base(node, run_config, print); // create an service client service_client_ = node_->create_client(service_name); // wait for action server - RCLCPP_INFO(node_->get_logger(), "%s waiting for service: %s", run_config_.interface.c_str(), service_name.c_str()); + info_("waiting for service: " + service_name); if (!service_client_->wait_for_service(std::chrono::seconds(3))) { - RCLCPP_ERROR(node_->get_logger(), "%s failed to connect to service: %s", run_config_.interface.c_str(), - service_name.c_str()); + error_("failed to connect to service: " + service_name); throw runner_exception("failed to connect to server"); } + + info_("connected with service: " + service_name); } /** @@ -67,39 +68,54 @@ class ServiceRunner : public RunnerBase throw runner_exception("cannot trigger service without parameters"); // generate a goal from parameters if provided - auto request_msg = std::make_shared(generate_request(parameters_[id])); + auto request_msg = std::make_shared(generate_request(parameters_[id], id)); + + info_("request generated", id); + + std::unique_lock lock(send_goal_mutex); + action_complete = false; auto result_future = service_client_->async_send_request( - request_msg, [this](typename rclcpp::Client::SharedFuture future) { + request_msg, [this, id](typename rclcpp::Client::SharedFuture future) { if (!future.valid()) { - RCLCPP_ERROR(node_->get_logger(), "get result call failed"); + error_("get result call failed"); // trigger the events related to on_failure state if (events[execute_id].on_failure != "") { + info_("on_failure", id, events[execute_id].on_failure, EventType::FAILED); triggerFunction_(events[execute_id].on_failure, update_on_failure(events[execute_id].on_failure_param)); } } else { - RCLCPP_INFO(node_->get_logger(), "get result call succeeded"); + info_("get result call succeeded", id); response_ = future.get(); // trigger the events related to on_success state if (events[execute_id].on_success != "") { + info_("on_success", id, events[execute_id].on_success, EventType::SUCCEEDED); triggerFunction_(events[execute_id].on_success, update_on_success(events[execute_id].on_success_param)); } } + + action_complete = true; + send_goal_cv.notify_all(); }); // trigger the events related to on_started state if (events[execute_id].on_started != "") { + info_("on_started", id, events[execute_id].on_started, EventType::STARTED); triggerFunction_(events[execute_id].on_started, update_on_started(events[execute_id].on_started_param)); } + + // Conditional wait + send_goal_cv.wait(lock, [this] { return action_complete; }); + info_("Service request complete. Thread closing.", id); } /** @@ -123,6 +139,7 @@ class ServiceRunner : public RunnerBase // Trigger on_stopped event if defined if (events[execute_id].on_stopped != "") { + info_("on_stopped", -1, events[execute_id].on_stopped, EventType::STOPPED); triggerFunction_(events[execute_id].on_stopped, update_on_stopped(events[execute_id].on_stopped_param)); } } @@ -139,7 +156,7 @@ class ServiceRunner : public RunnerBase * @param parameters * @return ServiceT::Request the generated request */ - virtual typename ServiceT::Request generate_request(tinyxml2::XMLElement* parameters) = 0; + virtual typename ServiceT::Request generate_request(tinyxml2::XMLElement* parameters, int id) = 0; typename rclcpp::Client::SharedPtr service_client_; typename ServiceT::Response::SharedPtr response_; diff --git a/capabilities2_runner/include/capabilities2_runner/topic_runner.hpp b/capabilities2_runner/include/capabilities2_runner/topic_runner.hpp index 2450851..868c401 100644 --- a/capabilities2_runner/include/capabilities2_runner/topic_runner.hpp +++ b/capabilities2_runner/include/capabilities2_runner/topic_runner.hpp @@ -33,10 +33,10 @@ class TopicRunner : public RunnerBase * @param topic_name topic name used in the yaml file, used to load specific configuration from the run_config */ virtual void init_subscriber(rclcpp::Node::SharedPtr node, const runner_opts& run_config, - const std::string& topic_name) + const std::string& topic_name, std::function print) { // initialize the runner base by storing node pointer and run config - init_base(node, run_config); + init_base(node, run_config, print); // create an service client subscription_ = node_->create_subscription( @@ -61,6 +61,7 @@ class TopicRunner : public RunnerBase // trigger the events related to on_started state if (events[execute_id].on_started != "") { + info_("on_started", id, events[execute_id].on_started, EventType::STARTED); triggerFunction_(events[execute_id].on_started, update_on_started(events[execute_id].on_started_param)); } @@ -69,16 +70,18 @@ class TopicRunner : public RunnerBase // trigger the events related to on_success state if (events[execute_id].on_success != "") { + info_("on_success", id, events[execute_id].on_success, EventType::SUCCEEDED); triggerFunction_(events[execute_id].on_success, update_on_success(events[execute_id].on_success_param)); } } else { - RCLCPP_ERROR(node_->get_logger(), "get result call failed"); + error_("get result call failed"); // trigger the events related to on_failure state if (events[execute_id].on_failure != "") { + info_("on_failure", id, events[execute_id].on_failure, EventType::FAILED); triggerFunction_(events[execute_id].on_failure, update_on_failure(events[execute_id].on_failure_param)); } } @@ -105,6 +108,7 @@ class TopicRunner : public RunnerBase // Trigger on_stopped event if defined if (events[execute_id].on_stopped != "") { + info_("on_stopped", -1, events[execute_id].on_stopped, EventType::STOPPED); triggerFunction_(events[execute_id].on_stopped, update_on_stopped(events[execute_id].on_stopped_param)); } } diff --git a/capabilities2_runner/include/capabilities2_runner/utils/capability_event_t.hpp b/capabilities2_runner/include/capabilities2_runner/utils/capability_event_t.hpp new file mode 100644 index 0000000..afe4dde --- /dev/null +++ b/capabilities2_runner/include/capabilities2_runner/utils/capability_event_t.hpp @@ -0,0 +1,13 @@ +#pragma once + +namespace capabilities2_runner +{ +enum capability_event +{ + IDLE, + STARTED, + STOPPED, + FAILED, + SUCCEEDED +}; +} \ No newline at end of file diff --git a/capabilities2_runner/src/standard_runners.cpp b/capabilities2_runner/src/standard_runners.cpp index 58ca354..e62d9e0 100644 --- a/capabilities2_runner/src/standard_runners.cpp +++ b/capabilities2_runner/src/standard_runners.cpp @@ -7,10 +7,11 @@ namespace capabilities2_runner class DummyRunner : public RunnerBase { public: - void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config) override + void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config, + std::function runner_publish_func) override { // init the base runner - init_base(node, run_config); + init_base(node, run_config, runner_publish_func); // do nothing RCLCPP_INFO(node_->get_logger(), "Dummy runner started"); 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 84fd7a7..62607a1 100644 --- a/capabilities2_runner_audio/include/capabilities2_runner_audio/listen_runner.hpp +++ b/capabilities2_runner_audio/include/capabilities2_runner_audio/listen_runner.hpp @@ -31,9 +31,10 @@ class ListenerRunner : public ActionRunner * @param node shared pointer to the capabilities node. Allows to use ros node related functionalities * @param run_config runner configuration loaded from the yaml file */ - virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config) override + virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config, + std::function runner_publish_func) override { - init_action(node, run_config, "speech_to_text"); + init_action(node, run_config, "speech_to_text", runner_publish_func); } protected: @@ -44,7 +45,7 @@ class ListenerRunner : public ActionRunner * provider=ListenerRunner/>' * @return ActionT::Goal the generated goal */ - virtual hri_audio_msgs::action::SpeechToText::Goal generate_goal(tinyxml2::XMLElement* parameters) override + virtual hri_audio_msgs::action::SpeechToText::Goal generate_goal(tinyxml2::XMLElement* parameters, int id) override { hri_audio_msgs::action::SpeechToText::Goal goal_msg; diff --git a/capabilities2_runner_audio/include/capabilities2_runner_audio/speak_runner.hpp b/capabilities2_runner_audio/include/capabilities2_runner_audio/speak_runner.hpp index 014e953..f4fd976 100644 --- a/capabilities2_runner_audio/include/capabilities2_runner_audio/speak_runner.hpp +++ b/capabilities2_runner_audio/include/capabilities2_runner_audio/speak_runner.hpp @@ -31,21 +31,23 @@ class SpeakerRunner : public ActionRunner * @param node shared pointer to the capabilities node. Allows to use ros node related functionalities * @param run_config runner configuration loaded from the yaml file */ - virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config) override + virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config, + std::function runner_publish_func) override { - init_action(node, run_config, "text_to_speech"); + init_action(node, run_config, "text_to_speech", runner_publish_func); } protected: /** * @brief This generate goal function overrides the generate_goal() function from ActionRunner() - * - * @param parameters XMLElement that contains parameters in the format '' + * + * @param parameters XMLElement that contains parameters in the format '' * @return ActionT::Goal the generated goal */ - virtual hri_audio_msgs::action::TextToSpeech::Goal generate_goal(tinyxml2::XMLElement* parameters) override + virtual hri_audio_msgs::action::TextToSpeech::Goal generate_goal(tinyxml2::XMLElement* parameters, int id) override { - const char **text; + const char** text; parameters->QueryStringAttribute("text", text); std::string tts_text(*text); @@ -69,7 +71,6 @@ class SpeakerRunner : public ActionRunner std::string feedback = ""; return feedback; } - }; } // namespace capabilities2_runner \ No newline at end of file diff --git a/capabilities2_runner_audio/src/audio_runners.cpp b/capabilities2_runner_audio/src/audio_runners.cpp index 559be66..5fb11f4 100644 --- a/capabilities2_runner_audio/src/audio_runners.cpp +++ b/capabilities2_runner_audio/src/audio_runners.cpp @@ -3,11 +3,6 @@ #include #include -namespace capabilities2_runner -{ - -} - // register runner plugins PLUGINLIB_EXPORT_CLASS(capabilities2_runner::ListenerRunner, capabilities2_runner::RunnerBase) PLUGINLIB_EXPORT_CLASS(capabilities2_runner::SpeakerRunner, capabilities2_runner::RunnerBase) diff --git a/capabilities2_runner_bt/include/capabilities2_runner_bt/bt_factory_runner.hpp b/capabilities2_runner_bt/include/capabilities2_runner_bt/bt_factory_runner.hpp index 846f1a8..c760350 100644 --- a/capabilities2_runner_bt/include/capabilities2_runner_bt/bt_factory_runner.hpp +++ b/capabilities2_runner_bt/include/capabilities2_runner_bt/bt_factory_runner.hpp @@ -21,10 +21,11 @@ class BTRunnerBase : public RunnerBase, public BT::BehaviorTreeFactory { } - virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config) + virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config, + std::function print) { // init the runner base - init_base(node, run_config); + init_base(node, run_config, print); // register (bt)actions from ROS plugins try diff --git a/capabilities2_runner_bt/src/bt_runners.cpp b/capabilities2_runner_bt/src/bt_runners.cpp index 3a630db..2f1582c 100644 --- a/capabilities2_runner_bt/src/bt_runners.cpp +++ b/capabilities2_runner_bt/src/bt_runners.cpp @@ -1,9 +1,5 @@ #include #include -namespace capabilities2_runner -{ - -} // namespace capabilities2_runner // PLUGINLIB_EXPORT_CLASS(capabilities2_runner::BTRunnerBase, capabilities2_runner::RunnerBase) diff --git a/capabilities2_runner_nav2/include/capabilities2_runner_nav2/occupancygrid_runner.hpp b/capabilities2_runner_nav2/include/capabilities2_runner_nav2/occupancygrid_runner.hpp index 6229a76..d95b9f3 100644 --- a/capabilities2_runner_nav2/include/capabilities2_runner_nav2/occupancygrid_runner.hpp +++ b/capabilities2_runner_nav2/include/capabilities2_runner_nav2/occupancygrid_runner.hpp @@ -29,13 +29,13 @@ class OccupancyGridRunner : public TopicRunner * @param node shared pointer to the capabilities node. Allows to use ros node related functionalities * @param run_config runner configuration loaded from the yaml file */ - virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config) override + virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config, + std::function print) override { - init_subscriber(node, run_config, "map"); + init_subscriber(node, run_config, "map", print); } protected: - /** * @brief Update on_success event parameters with new data if avaible. * @@ -145,8 +145,9 @@ class OccupancyGridRunner : public TopicRunner occupancyGridElement->InsertEndChild(dataElement); std::string data_str; - for (size_t i = 0; i < latest_message_->data.size(); ++i) { - data_str += std::to_string(latest_message_->data[i]) + " "; + for (size_t i = 0; i < latest_message_->data.size(); ++i) + { + data_str += std::to_string(latest_message_->data[i]) + " "; } dataElement->SetText(data_str.c_str()); diff --git a/capabilities2_runner_nav2/include/capabilities2_runner_nav2/robotpose_runner.hpp b/capabilities2_runner_nav2/include/capabilities2_runner_nav2/robotpose_runner.hpp index 763d5a5..c68ccfd 100644 --- a/capabilities2_runner_nav2/include/capabilities2_runner_nav2/robotpose_runner.hpp +++ b/capabilities2_runner_nav2/include/capabilities2_runner_nav2/robotpose_runner.hpp @@ -29,9 +29,10 @@ class RobotPoseRunner : public TopicRunner runner_publish_func) override { - init_subscriber(node, run_config, "pose"); + init_subscriber(node, run_config, "pose", runner_publish_func); } 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 a28a2da..dc7a14d 100644 --- a/capabilities2_runner_nav2/include/capabilities2_runner_nav2/waypoint_runner.hpp +++ b/capabilities2_runner_nav2/include/capabilities2_runner_nav2/waypoint_runner.hpp @@ -37,9 +37,10 @@ class WayPointRunner : public ActionRunner * @param node shared pointer to the capabilities node. Allows to use ros node related functionalities * @param run_config runner configuration loaded from the yaml file */ - virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config) override + virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config, + std::function runner_publish_func) override { - init_action(node, run_config, "navigate_to_pose"); + init_action(node, run_config, "navigate_to_pose", runner_publish_func); } protected: @@ -49,12 +50,12 @@ class WayPointRunner : public ActionRunner '' * @return ActionT::Goal the generated goal */ - virtual nav2_msgs::action::NavigateToPose::Goal generate_goal(tinyxml2::XMLElement* parameters) override + virtual nav2_msgs::action::NavigateToPose::Goal generate_goal(tinyxml2::XMLElement* parameters, int id) override { parameters->QueryDoubleAttribute("x", &x); parameters->QueryDoubleAttribute("y", &y); - RCLCPP_INFO(node_->get_logger(), "[%s] goal consist of x: %f and y: %f", run_config_.interface.c_str(), x, y); + info_("goal consist of x: " + std::to_string(x) + " and y: " + std::to_string(y), id); nav2_msgs::action::NavigateToPose::Goal goal_msg; geometry_msgs::msg::PoseStamped pose_msg; @@ -62,7 +63,7 @@ class WayPointRunner : public ActionRunner pose_msg.header.frame_id = "map"; pose_msg.header.stamp.sec = 0; pose_msg.header.stamp.nanosec = 0; - + pose_msg.pose.position.x = x; pose_msg.pose.position.y = y; pose_msg.pose.position.z = 0.0; diff --git a/capabilities2_runner_nav2/src/nav2_runners.cpp b/capabilities2_runner_nav2/src/nav2_runners.cpp index c5387c5..d0ea3f1 100644 --- a/capabilities2_runner_nav2/src/nav2_runners.cpp +++ b/capabilities2_runner_nav2/src/nav2_runners.cpp @@ -4,11 +4,6 @@ #include #include -namespace capabilities2_runner -{ - -} - // register runner plugins PLUGINLIB_EXPORT_CLASS(capabilities2_runner::WayPointRunner, capabilities2_runner::RunnerBase) PLUGINLIB_EXPORT_CLASS(capabilities2_runner::RobotPoseRunner, capabilities2_runner::RunnerBase) diff --git a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_occupancy_runner.hpp b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_occupancy_runner.hpp index cea6c6d..af21915 100644 --- a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_occupancy_runner.hpp +++ b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_occupancy_runner.hpp @@ -28,9 +28,10 @@ class PromptOccupancyRunner : public ServiceRunner * @param node shared pointer to the capabilities node. Allows to use ros node related functionalities * @param run_config runner configuration loaded from the yaml file */ - virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config) override + virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config, + std::function print) override { - init_service(node, run_config, "prompt"); + init_service(node, run_config, "prompt", print); } /** @@ -44,7 +45,7 @@ class PromptOccupancyRunner : public ServiceRunner * @param parameters * @return prompt_msgs::srv::Prompt::Request the generated request */ - virtual typename prompt_msgs::srv::Prompt::Request generate_request(tinyxml2::XMLElement* parameters) override + virtual typename prompt_msgs::srv::Prompt::Request generate_request(tinyxml2::XMLElement* parameters, int id) override { tinyxml2::XMLElement* occupancyElement = parameters->FirstChildElement("OccupancyGrid"); @@ -55,7 +56,9 @@ class PromptOccupancyRunner : public ServiceRunner prompt_msgs::srv::Prompt::Request request; - request.prompt.prompt = "The OccupancyGrid of the robot shows the surrounding environment of the robot. The data is given as a ros2 nav_msgs occupancy_grid of which the content are " + data; + request.prompt.prompt = "The OccupancyGrid of the robot shows the surrounding environment of the robot. The data " + "is given as a ros2 nav_msgs occupancy_grid of which the content are " + + data; prompt_msgs::msg::ModelOption modelOption1; modelOption1.key = "model"; diff --git a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_request_runner.hpp b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_request_runner.hpp index b75c5e5..62935ac 100644 --- a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_request_runner.hpp +++ b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_request_runner.hpp @@ -28,9 +28,10 @@ class PromptPlanRequestRunner : public ServiceRunner * @param node shared pointer to the capabilities node. Allows to use ros node related functionalities * @param run_config runner configuration loaded from the yaml file */ - virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config) override + virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config, + std::function print) override { - init_service(node, run_config, "prompt"); + init_service(node, run_config, "prompt", print); } protected: @@ -45,7 +46,7 @@ class PromptPlanRequestRunner : public ServiceRunner * @param parameters * @return prompt_msgs::srv::Prompt::Request the generated request */ - virtual typename prompt_msgs::srv::Prompt::Request generate_request(tinyxml2::XMLElement* parameters) override + virtual typename prompt_msgs::srv::Prompt::Request generate_request(tinyxml2::XMLElement* parameters, int id) override { bool replan; parameters->QueryBoolAttribute("replan", &replan); diff --git a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_response_runner.hpp b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_response_runner.hpp index d6c3a36..784c52d 100644 --- a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_response_runner.hpp +++ b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_response_runner.hpp @@ -31,9 +31,10 @@ class PromptPlanResponseRunner : public ActionRunner print) override { - init_action(node, run_config, "capabilities_fabric"); + init_action(node, run_config, "capabilities_fabric", print); } protected: @@ -43,7 +44,7 @@ class PromptPlanResponseRunner : public ActionRunner' * @return ActionT::Goal the generated goal */ - virtual capabilities2_msgs::action::Plan::Goal generate_goal(tinyxml2::XMLElement* parameters) override + virtual capabilities2_msgs::action::Plan::Goal generate_goal(tinyxml2::XMLElement* parameters, int id) override { tinyxml2::XMLElement* planElement = parameters->FirstChildElement("ReceievdPlan"); diff --git a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_pose_runner.hpp b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_pose_runner.hpp index 3dda94a..b7cb004 100644 --- a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_pose_runner.hpp +++ b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_pose_runner.hpp @@ -28,9 +28,10 @@ class PromptPoseRunner : public ServiceRunner * @param node shared pointer to the capabilities node. Allows to use ros node related functionalities * @param run_config runner configuration loaded from the yaml file */ - virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config) override + virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config, + std::function print) override { - init_service(node, run_config, "prompt"); + init_service(node, run_config, "prompt", print); } /** @@ -44,7 +45,7 @@ class PromptPoseRunner : public ServiceRunner * @param parameters * @return prompt_msgs::srv::Prompt::Request the generated request */ - virtual typename prompt_msgs::srv::Prompt::Request generate_request(tinyxml2::XMLElement* parameters) override + virtual typename prompt_msgs::srv::Prompt::Request generate_request(tinyxml2::XMLElement* parameters, int id) override { tinyxml2::XMLElement* poseElement = parameters->FirstChildElement("Pose"); @@ -55,7 +56,8 @@ class PromptPoseRunner : public ServiceRunner prompt_msgs::srv::Prompt::Request request; - request.prompt.prompt = "The position of the robot is given as a standard ros2 geometry message of which the content are " + data; + request.prompt.prompt = + "The position of the robot is given as a standard ros2 geometry message of which the content are " + data; prompt_msgs::msg::ModelOption modelOption1; modelOption1.key = "model"; diff --git a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_text_runner.hpp b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_text_runner.hpp index a418f76..6262cc7 100644 --- a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_text_runner.hpp +++ b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_text_runner.hpp @@ -28,9 +28,10 @@ class PromptTextRunner : public ServiceRunner * @param node shared pointer to the capabilities node. Allows to use ros node related functionalities * @param run_config runner configuration loaded from the yaml file */ - virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config) override + virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config, + std::function print) override { - init_service(node, run_config, "prompt"); + init_service(node, run_config, "prompt", print); } /** @@ -44,7 +45,7 @@ class PromptTextRunner : public ServiceRunner * @param parameters * @return prompt_msgs::srv::Prompt::Request the generated request */ - virtual typename prompt_msgs::srv::Prompt::Request generate_request(tinyxml2::XMLElement* parameters) override + virtual typename prompt_msgs::srv::Prompt::Request generate_request(tinyxml2::XMLElement* parameters, int id) override { tinyxml2::XMLElement* textElement = parameters->FirstChildElement("Text"); diff --git a/capabilities2_runner_prompt/src/prompt_runners.cpp b/capabilities2_runner_prompt/src/prompt_runners.cpp index 32bc8c4..361733f 100644 --- a/capabilities2_runner_prompt/src/prompt_runners.cpp +++ b/capabilities2_runner_prompt/src/prompt_runners.cpp @@ -6,11 +6,6 @@ #include #include -namespace capabilities2_runner -{ - -} - // register runner plugins PLUGINLIB_EXPORT_CLASS(capabilities2_runner::PromptTextRunner, capabilities2_runner::RunnerBase) PLUGINLIB_EXPORT_CLASS(capabilities2_runner::PromptPoseRunner, capabilities2_runner::RunnerBase) diff --git a/capabilities2_server/include/capabilities2_server/capabilities_api.hpp b/capabilities2_server/include/capabilities2_server/capabilities_api.hpp index 3d19640..e95cafb 100644 --- a/capabilities2_server/include/capabilities2_server/capabilities_api.hpp +++ b/capabilities2_server/include/capabilities2_server/capabilities_api.hpp @@ -11,10 +11,12 @@ #include #include + #include #include #include #include +#include #include #include @@ -26,15 +28,6 @@ namespace capabilities2_server { -enum capability_event -{ - IDLE, - STARTED, - STOPPED, - TERMINATED, - SUCCEEDED -}; - /** * @brief capabilities api * mapping message based logic of capabilities to database models @@ -55,38 +48,22 @@ class CapabilitiesAPI * @param db_file file path of the database file * @param node_logging_interface_ptr pointer to the ROS node logging interface */ - void connect(const std::string& db_file, - rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_interface_ptr) + void connect(const std::string& db_file, rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logging, + std::function print, + std::function runner_print) { - // set logger - node_logging_interface_ptr_ = node_logging_interface_ptr; + print_ = print; + logging_ = logging; - runner_cache_.connect(node_logging_interface_ptr); + runner_cache_.connect(print, runner_print, logging); // connect db cap_db_ = std::make_unique(db_file); // log - RCLCPP_INFO(node_logging_interface_ptr_->get_logger(), "CAPAPI connected to db: %s", db_file.c_str()); + print_("Capabilities API connected to db: " + db_file, false, false); } - /** - * @brief initialize runner events. THese are called from within runners and passed on to those execution levels - * as function pointers - * - * @param on_started event triggered at the start of the runner - * @param on_stopped event triggered at the shutdown of the runner by the capabilities server - * @param on_terminated event triggered at the failure of the runner by the action or server side. - */ - // void init_events(std::function on_started, - // std::function on_stopped, - // std::function on_terminated) - // { - // runner_cache_.set_on_started(on_started); - // runner_cache_.set_on_stopped(on_stopped); - // runner_cache_.set_on_terminated(on_terminated); - // } - /** * @brief Start a capability. Internal function only. Do not used this function externally. * @@ -108,7 +85,7 @@ class CapabilitiesAPI // go through the running model and start the necessary dependencies for (const auto& run : running.dependencies) { - RCLCPP_INFO(node_logging_interface_ptr_->get_logger(), "found dependency: %s", run.interface.c_str()); + print_("found dependency: " + run.interface, true, false); // make an internal 'use' bond for the capability dependency bind_dependency(run.interface); @@ -129,16 +106,13 @@ class CapabilitiesAPI { runner_cache_.add_runner(node, capability, run_config); - // log - RCLCPP_INFO(node_logging_interface_ptr_->get_logger(), "started capability: %s with provider: %s", - capability.c_str(), provider.c_str()); + print_("started capability: " + capability + " with provider: " + provider, true, false); return value and true; } catch (const capabilities2_runner::runner_exception& e) { - RCLCPP_WARN(node_logging_interface_ptr_->get_logger(), "could not start runner: %s", e.what()); - + RCLCPP_WARN(logging_->get_logger(), "could not start runner: %s", e.what()); return false; } } @@ -161,7 +135,7 @@ class CapabilitiesAPI } catch (const capabilities2_runner::runner_exception& e) { - RCLCPP_WARN(node_logging_interface_ptr_->get_logger(), "could not trigger runner: %s", e.what()); + RCLCPP_WARN(logging_->get_logger(), "could not trigger runner: %s", e.what()); } } @@ -176,7 +150,7 @@ class CapabilitiesAPI // this can happen if dependencies fail to resolve in the first place if (!runner_cache_.running(capability)) { - RCLCPP_ERROR(node_logging_interface_ptr_->get_logger(), "could not get provider for: %s", capability.c_str()); + print_("could not get provider for: " + capability, true, true); return; } @@ -190,7 +164,7 @@ class CapabilitiesAPI // FIXME: this unrolls the dependency tree from the bottom up but should probably be top down for (const auto& run : running.dependencies) { - RCLCPP_INFO(node_logging_interface_ptr_->get_logger(), "freeing dependency: %s", run.interface.c_str()); + print_("freeing dependency: " + run.interface, true, false); // remove the internal 'use' bond for the capability dependency unbind_dependency(run.interface); @@ -210,12 +184,12 @@ class CapabilitiesAPI } catch (const capabilities2_runner::runner_exception& e) { - RCLCPP_WARN(node_logging_interface_ptr_->get_logger(), "could not stop runner: %s", e.what()); + RCLCPP_WARN(logging_->get_logger(), "could not stop runner: %s", e.what()); return; } // log - RCLCPP_INFO(node_logging_interface_ptr_->get_logger(), "stopped capability: %s", capability.c_str()); + print_("stopped capability: " + capability, true, false); } /** @@ -234,7 +208,8 @@ class CapabilitiesAPI if (!bond_cache_.exists(capability)) { // stop the capability - RCLCPP_INFO(node_logging_interface_ptr_->get_logger(), "stopping freed capability: %s", capability.c_str()); + print_("stopping freed capability: " + capability, true, false); + stop_capability(capability); } } @@ -282,18 +257,17 @@ class CapabilitiesAPI try { // log - RCLCPP_INFO(node_logging_interface_ptr_->get_logger(), ""); - RCLCPP_INFO(node_logging_interface_ptr_->get_logger(), "setting triggers for capability: %s", capability.c_str()); + print_("Setting triggers for capability: " + capability, true, false); runner_cache_.set_runner_triggers(capability, on_started_capability, on_started_parameters, on_failure_capability, on_failure_parameters, on_success_capability, on_success_parameters, on_stopped_capability, on_stopped_parameters); - RCLCPP_INFO(node_logging_interface_ptr_->get_logger(), "successfully set triggers"); + print_("Successfully set triggers for capability: " + capability, true, false); } catch (const capabilities2_runner::runner_exception& e) { - RCLCPP_WARN(node_logging_interface_ptr_->get_logger(), "could not set triggers: %s", e.what()); + RCLCPP_WARN(logging_->get_logger(), "Sould not set triggers: %s", e.what()); } } @@ -311,7 +285,7 @@ class CapabilitiesAPI // exists guard if (cap_db_->exists(header.name)) { - RCLCPP_WARN(node_logging_interface_ptr_->get_logger(), "interface already exists"); + RCLCPP_WARN(logging_->get_logger(), "interface already exists"); return; } @@ -323,7 +297,7 @@ class CapabilitiesAPI } catch (const std::exception& e) { - RCLCPP_ERROR(node_logging_interface_ptr_->get_logger(), "failed to convert spec to model: %s", e.what()); + print_("failed to convert spec to model: " + std::string(e.what()), true, true); return; } @@ -331,8 +305,8 @@ class CapabilitiesAPI model.header.name = spec.package + "/" + model.header.name; cap_db_->insert_interface(model); - // log - RCLCPP_INFO(node_logging_interface_ptr_->get_logger(), "interface added to db: %s", model.header.name.c_str()); + print_("interface added to db: " + model.header.name, true, false); + return; } @@ -340,7 +314,7 @@ class CapabilitiesAPI { if (cap_db_->exists(header.name)) { - RCLCPP_WARN(node_logging_interface_ptr_->get_logger(), "semantic interface already exists"); + RCLCPP_WARN(logging_->get_logger(), "semantic interface already exists"); return; } @@ -352,7 +326,7 @@ class CapabilitiesAPI } catch (const std::exception& e) { - RCLCPP_ERROR(node_logging_interface_ptr_->get_logger(), "failed to convert spec to model: %s", e.what()); + print_("failed to convert spec to model: " + std::string(e.what()), true, true); return; } @@ -360,8 +334,8 @@ class CapabilitiesAPI cap_db_->insert_semantic_interface(model); // log - RCLCPP_INFO(node_logging_interface_ptr_->get_logger(), "semantic interface added to db: %s", - model.header.name.c_str()); + print_("semantic interface added to db: " + model.header.name, true, false); + return; } @@ -369,7 +343,7 @@ class CapabilitiesAPI { if (cap_db_->exists(header.name)) { - RCLCPP_WARN(node_logging_interface_ptr_->get_logger(), "provider already exists"); + RCLCPP_WARN(logging_->get_logger(), "provider already exists"); return; } @@ -381,19 +355,20 @@ class CapabilitiesAPI } catch (const std::exception& e) { - RCLCPP_ERROR(node_logging_interface_ptr_->get_logger(), "failed to convert spec to model: %s", e.what()); + print_("failed to convert spec to model: " + std::string(e.what()), true, true); return; } + model.header.name = spec.package + "/" + model.header.name; cap_db_->insert_provider(model); // log - RCLCPP_INFO(node_logging_interface_ptr_->get_logger(), "provider added to db: %s", model.header.name.c_str()); + print_("provider added to db: " + model.header.name, true, false); return; } // couldn't parse unknown capability type - RCLCPP_ERROR(node_logging_interface_ptr_->get_logger(), "unknown capability type: %s", spec.type.c_str()); + print_("unknown capability type: " +spec.type, true, true); } // query api @@ -633,106 +608,6 @@ class CapabilitiesAPI return atoi(runner_cache_.pid(capability).c_str()); } - // event api - // related to runner api - // const capabilities2_msgs::msg::CapabilityEvent on_capability_started(const std::string& capability) - // { - // // create event msg - // capabilities2_msgs::msg::CapabilityEvent event; - // event.header.frame_id = "capabilities"; - // event.header.stamp = rclcpp::Clock().now(); - - // // started event - // event.type = capabilities2_msgs::msg::CapabilityEvent::LAUNCHED; - - // // set cap, prov, pid - // event.capability = capability; - - // // try to get details - // try - // { - // event.provider = runner_cache_.provider(capability); - // event.pid = atoi(runner_cache_.pid(capability).c_str()); - // } - // catch (const capabilities2_runner::runner_exception& e) - // { - // event.provider = "unknown"; - // event.pid = -1; - // } - - // return event; - // } - - // const capabilities2_msgs::msg::CapabilityEvent on_capability_stopped(const std::string& capability) - // { - // // create event msg - // capabilities2_msgs::msg::CapabilityEvent event; - // event.header.frame_id = "capabilities"; - // event.header.stamp = rclcpp::Clock().now(); - - // // terminated event - // event.type = capabilities2_msgs::msg::CapabilityEvent::STOPPED; - - // // set cap, prov, pid - // event.capability = capability; - - // // try to get details - // try - // { - // event.provider = runner_cache_.provider(capability); - // event.pid = atoi(runner_cache_.pid(capability).c_str()); - // } - // catch (const capabilities2_runner::runner_exception& e) - // { - // event.provider = "unknown"; - // event.pid = -1; - // } - - // return event; - // } - - // const capabilities2_msgs::msg::CapabilityEvent on_capability_terminated(const std::string& capability) - // { - // // create event msg - // capabilities2_msgs::msg::CapabilityEvent event; - // event.header.frame_id = "capabilities"; - // event.header.stamp = rclcpp::Clock().now(); - - // // terminated event - // event.type = capabilities2_msgs::msg::CapabilityEvent::TERMINATED; - - // // set cap, prov, pid - // event.capability = capability; - // try - // { - // event.provider = runner_cache_.provider(capability); - // event.pid = atoi(runner_cache_.pid(capability).c_str()); - // } - // catch (const capabilities2_runner::runner_exception& e) - // { - // event.provider = "unknown"; - // event.pid = -1; - // } - - // // log error - // RCLCPP_ERROR(node_logging_interface_ptr_->get_logger(), "capability terminated: %s", capability.c_str()); - - // return event; - // } - - // const capabilities2_msgs::msg::CapabilityEvent on_server_ready() - // { - // // create event msg - // capabilities2_msgs::msg::CapabilityEvent event; - // event.header.frame_id = "capabilities"; - // event.header.stamp = rclcpp::Clock().now(); - - // // started event - // event.type = capabilities2_msgs::msg::CapabilityEvent::SERVER_READY; - - // return event; - // } - // bond api // establish bond const std::string establish_bond(rclcpp::Node::SharedPtr node) @@ -752,13 +627,13 @@ class CapabilitiesAPI void on_bond_established(const std::string& bond_id) { // log bond established event - RCLCPP_INFO(node_logging_interface_ptr_->get_logger(), "bond established with id: %s", bond_id.c_str()); + RCLCPP_INFO(logging_->get_logger(), "bond established with id: %s", bond_id.c_str()); } void on_bond_broken(const std::string& bond_id) { // log warning - RCLCPP_WARN(node_logging_interface_ptr_->get_logger(), "bond broken for id: %s", bond_id.c_str()); + RCLCPP_WARN(logging_->get_logger(), "bond broken for id: %s", bond_id.c_str()); // get capabilities requested by the bond std::vector capabilities = bond_cache_.get_capabilities(bond_id); @@ -829,18 +704,21 @@ class CapabilitiesAPI } private: - // logger - rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_interface_ptr_; - // db std::unique_ptr cap_db_; + // for internal logging + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logging_; + // caches BondCache bond_cache_; RunnerCache runner_cache_; // internal bindings std::vector internal_bond_ids_; + + // event function for external event publishing + std::function print_; }; } // namespace capabilities2_server diff --git a/capabilities2_server/include/capabilities2_server/capabilities_server.hpp b/capabilities2_server/include/capabilities2_server/capabilities_server.hpp index 6e6b2b5..1d4187e 100644 --- a/capabilities2_server/include/capabilities2_server/capabilities_server.hpp +++ b/capabilities2_server/include/capabilities2_server/capabilities_server.hpp @@ -89,11 +89,11 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI if (rebuild) { // remove db file if it exists - event_publish("Rebuilding capabilities database", false); + event_publish("Removing the old capabilities database", false); if (std::remove(db_file.c_str()) != 0) { - RCLCPP_ERROR(get_logger(), "Error deleting file %s", db_file.c_str()); + event_publish("Error deleting database file " + db_file, false, true); } } @@ -109,7 +109,10 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI // init capabilities api event_publish("Connecting API with Database", false); - connect(db_file, get_node_logging_interface()); + connect(db_file, get_node_logging_interface(), + std::bind(&capabilities2_server::CapabilitiesServer::event_publish, this, std::placeholders::_1, + std::placeholders::_2, std::placeholders::_3), + std::bind(&capabilities2_server::CapabilitiesServer::event_publish_runner, this, std::placeholders::_1)); event_publish("Loading capabilities", false); @@ -248,13 +251,13 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI // guard empty values if (req->capability.empty()) { - RCLCPP_ERROR(get_logger(), "free_capability: capability is empty"); + event_publish("free_capability: capability is empty", true, true); return; } if (req->bond_id.empty()) { - RCLCPP_ERROR(get_logger(), "free_capability: bond_id is empty"); + event_publish("free_capability: bond_id is empty", true, true); return; } @@ -271,19 +274,19 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI // guard empty values if (req->capability.empty()) { - RCLCPP_ERROR(get_logger(), "use_capability: capability is empty"); + event_publish("use_capability: capability is empty", true, true); return; } if (req->preferred_provider.empty()) { - RCLCPP_ERROR(get_logger(), "use_capability: preferred_provider is empty"); + event_publish("use_capability: preferred_provider is empty", true, true); return; } if (req->bond_id.empty()) { - RCLCPP_ERROR(get_logger(), "use_capability: bond_id is empty"); + event_publish("use_capability: bond_id is empty", true, true); return; } @@ -313,7 +316,7 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI // guard empty values if (req->capability_spec.package.empty() || req->capability_spec.content.empty()) { - RCLCPP_ERROR(get_logger(), "register_capability: package or content is empty"); + event_publish("register_capability: package or content is empty", true, true); return; } @@ -363,7 +366,7 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI // if the spec is not empty set response if (capability_spec.content.empty()) { - RCLCPP_ERROR(get_logger(), "capability spec not found for resource: %s", req->capability_spec.c_str()); + event_publish("capability spec not found for resource: " + req->capability_spec, true, true); // BUG: throw error causes service to crash, this is a ROS2 bug // std::runtime_error("capability spec not found for resource: " + req->capability_spec); @@ -411,7 +414,7 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI // check if path exists if (!std::filesystem::exists(package_path)) { - RCLCPP_ERROR(get_logger(), "package path does not exist: %s", package_path.c_str()); + event_publish("package path does not exist: " + package_path, true, true); return; } @@ -459,7 +462,7 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI } catch (const std::runtime_error& e) { - RCLCPP_ERROR(get_logger(), "failed to parse package.xml file: %s", e.what()); + event_publish("failed to parse package.xml file: " + std::string(e.what()), true, true); continue; } @@ -499,14 +502,13 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI load_spec_content(package_path + "/" + package + "/" + spec_path, capability_spec); // add capability to db - std::string status = "adding capability: " + package + "/" + spec_path; - event_publish(status); + event_publish("adding capability: " + package + "/" + spec_path); add_capability(capability_spec); } catch (const std::runtime_error& e) { - RCLCPP_ERROR(get_logger(), "failed to load spec file: %s", e.what()); + event_publish("failed to load spec file: " + std::string(e.what()), true, true); } } }; @@ -543,7 +545,7 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI } catch (const std::runtime_error& e) { - RCLCPP_ERROR(get_logger(), "failed to parse package.xml file: %s", e.what()); + event_publish("failed to parse package.xml file: " + std::string(e.what()), true, true); continue; } @@ -583,14 +585,13 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI load_spec_content(package_path + "/" + package + "/share/" + package + "/" + spec_path, capability_spec); // add capability to db - std::string status = "adding capability: " + package + "/" + spec_path; - event_publish(status); + event_publish("adding capability: " + package + "/" + spec_path); add_capability(capability_spec); } catch (const std::runtime_error& e) { - RCLCPP_ERROR(get_logger(), "failed to load spec file: %s", e.what()); + event_publish("failed to load spec file: " + std::string(e.what()), true, true); } } }; @@ -656,7 +657,7 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI spec_file_file.close(); } - void event_publish(const std::string& text, bool is_server_ready = true) + void event_publish(const std::string& text, bool is_server_ready = true, bool is_error = false) { auto message = capabilities2_msgs::msg::CapabilityEvent(); @@ -667,47 +668,48 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI message.target.provider = ""; message.thread_id = 0; message.text = text; + message.error = is_error; message.pid = -1; message.server_ready = is_server_ready; event_pub_->publish(message); - RCLCPP_INFO(get_logger(), text.c_str()); + if (is_error) + RCLCPP_ERROR(get_logger(), text.c_str()); + else + RCLCPP_INFO(get_logger(), text.c_str()); } - void event_publish_runner(const std::string& capability, const std::string& provider, int thread_id, - const std::string& text, const std::string target_capability = "", - const std::string target_provider = "", - enum capability_event event = capability_event::IDLE) + void event_publish_runner(capabilities2_msgs::msg::CapabilityEvent& message) { - auto message = capabilities2_msgs::msg::CapabilityEvent(); - - message.header.stamp = rclcpp::Clock().now(); - message.source.capability = capability; - message.source.provider = provider; - message.target.capability = target_capability; - message.target.provider = target_provider; - message.thread_id = thread_id; - message.text = text; - message.pid = get_pid(capability); + message.pid = get_pid(message.source.capability); message.server_ready = true; - if (event == capability_event::IDLE) - message.event = capabilities2_msgs::msg::CapabilityEvent::IDLE; - else if (event == capability_event::STARTED) - message.event = capabilities2_msgs::msg::CapabilityEvent::STARTED; - else if (event == capability_event::STOPPED) - message.event = capabilities2_msgs::msg::CapabilityEvent::STOPPED; - else if (event == capability_event::TERMINATED) - message.event = capabilities2_msgs::msg::CapabilityEvent::TERMINATED; - else if (event == capability_event::SUCCEEDED) - message.event = capabilities2_msgs::msg::CapabilityEvent::SUCCEEDED; - else - message.event = capabilities2_msgs::msg::CapabilityEvent::UNDEFINED; - event_pub_->publish(message); - RCLCPP_INFO(get_logger(), text.c_str()); + std::string text; + if (message.thread_id >= 0 and message.target.capability == "") + { + text = "[" + message.source.capability + "/" + std::to_string(message.thread_id) + "] " + message.text; + } + else if (message.thread_id < 0 and message.target.capability == "") + { + text = "[" + message.source.capability + "] " + message.text; + } + else if (message.thread_id >= 0 and message.target.capability != "") + { + text = "[" + message.source.capability + "/" + std::to_string(message.thread_id) + "] triggering " + + message.target.capability + " " + message.text; + } + else if (message.thread_id < 0 and message.target.capability != "") + { + text = "[" + message.source.capability + "] triggering " + message.target.capability + " " + message.text; + } + + if (message.error) + RCLCPP_ERROR(get_logger(), text.c_str()); + else + RCLCPP_INFO(get_logger(), text.c_str()); } private: diff --git a/capabilities2_server/include/capabilities2_server/runner_cache.hpp b/capabilities2_server/include/capabilities2_server/runner_cache.hpp index b702409..af1a2e3 100644 --- a/capabilities2_server/include/capabilities2_server/runner_cache.hpp +++ b/capabilities2_server/include/capabilities2_server/runner_cache.hpp @@ -38,12 +38,18 @@ class RunnerCache /** * @brief connect with ROS node logging interface * - * @param node_logging_interface_ptr pointer to the ROS node logging interface + * @param print function to publish INFO messages to the ROS environment + * @param runner_print function to be passed into runners for publishing INFO messages to the ROS environment + * @param logging pointer to the ROS node logging interface for WARN and ERROR messages */ - void connect(rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_interface_ptr) + void connect(std::function print, + std::function runner_print, + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logging) { // set logger - node_logging_interface_ptr_ = node_logging_interface_ptr; + logging_ = logging; + print_ = print; + runner_print_ = runner_print; } /** @@ -85,7 +91,7 @@ class RunnerCache } // start the runner - runner_cache_[capability]->start(node, run_config.to_runner_opts()); + runner_cache_[capability]->start(node, run_config.to_runner_opts(), runner_print_); } /** @@ -105,8 +111,7 @@ class RunnerCache } else { - RCLCPP_ERROR(node_logging_interface_ptr_->get_logger(), "Runner not found for capability: %s", - capability.c_str()); + print_("Runner not found for capability: " + capability, true, true); throw capabilities2_runner::runner_exception("capability runner not found: " + capability); } } @@ -147,11 +152,9 @@ class RunnerCache event_options, std::bind(&capabilities2_server::RunnerCache::trigger_runner, this, std::placeholders::_1, std::placeholders::_2)); - RCLCPP_INFO(node_logging_interface_ptr_->get_logger(), - "Configured triggers for capability %s: \n\tStarted: %s \n\tFailure: %s \n\tSuccess: %s \n\tStopped: " - "%s\n", - capability.c_str(), on_started_capability.c_str(), on_failure_capability.c_str(), - on_success_capability.c_str(), on_stopped_capability.c_str()); + print_("Configured triggers for capability" + capability + ": \n\tStarted: " + on_started_capability + + " \n\tFailure: " + on_failure_capability + " \n\tSuccess: " + on_success_capability + + "\n\tStopped: " + on_stopped_capability + "\n", true, false); } /** @@ -265,7 +268,16 @@ class RunnerCache pluginlib::ClassLoader runner_loader_; // logger - rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_interface_ptr_; + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logging_; + + // event function for external event publishing + std::function print_; + + // event function for internal runner event publishing + std::function runner_print_; + + // event string + std::string event; }; } // namespace capabilities2_server From ae618e804827559476e3b7723b2b9c0e7527b099 Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Tue, 4 Feb 2025 02:42:37 +1100 Subject: [PATCH 082/133] added capabilities_runners. integration not finished --- .../capabilities_fabric.hpp | 235 +++++++++++------- .../capabilities_fabric_client.hpp | 51 ++-- .../utils/fabric_status.hpp | 14 ++ capabilities2_msgs/CMakeLists.txt | 1 + capabilities2_msgs/srv/CompleteFabric.srv | 1 + .../capabilities2_runner/action_runner.hpp | 15 +- .../capabilities2_runner/runner_base.hpp | 14 +- .../capabilities2_runner/service_runner.hpp | 10 +- .../.clang-format | 64 +++++ .../CMakeLists.txt | 63 +++++ .../fabric_completion_runner.hpp | 53 ++++ capabilities2_runner_capabilities/package.xml | 28 +++ capabilities2_runner_capabilities/plugins.xml | 7 + .../src/capabilities2_runner.cpp | 6 + capabilities2_runner_nav2/CMakeLists.txt | 2 - .../waypoint_runner.hpp | 3 - 16 files changed, 428 insertions(+), 139 deletions(-) create mode 100644 capabilities2_fabric/include/capabilities2_fabric/utils/fabric_status.hpp create mode 100644 capabilities2_msgs/srv/CompleteFabric.srv create mode 100644 capabilities2_runner_capabilities/.clang-format create mode 100644 capabilities2_runner_capabilities/CMakeLists.txt create mode 100644 capabilities2_runner_capabilities/include/capabilities2_runner_capabilities/fabric_completion_runner.hpp create mode 100644 capabilities2_runner_capabilities/package.xml create mode 100644 capabilities2_runner_capabilities/plugins.xml create mode 100644 capabilities2_runner_capabilities/src/capabilities2_runner.cpp diff --git a/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric.hpp b/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric.hpp index e0b5f06..d2cb291 100644 --- a/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric.hpp +++ b/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric.hpp @@ -2,6 +2,7 @@ #include #include #include +#include #include #include @@ -11,6 +12,7 @@ #include #include #include +#include #include @@ -22,6 +24,7 @@ #include #include #include +#include /** * @brief Capabilities Fabric @@ -46,6 +49,8 @@ class CapabilitiesFabric : public rclcpp::Node using ConfigureCapability = capabilities2_msgs::srv::ConfigureCapability; using TriggerCapability = capabilities2_msgs::srv::TriggerCapability; + using CompleteFabric = capabilities2_msgs::srv::CompleteFabric; + using GetInterfacesClient = rclcpp::Client; using GetSemanticInterfacesClient = rclcpp::Client; using GetProvidersClient = rclcpp::Client; @@ -55,7 +60,9 @@ class CapabilitiesFabric : public rclcpp::Node using ConfigureCapabilityClient = rclcpp::Client; using TriggerCapabilityClient = rclcpp::Client; - CapabilitiesFabric(const rclcpp::NodeOptions& options = rclcpp::NodeOptions()) : Node("Capabilities2_Fabric", options) + using Status = capabilities2::fabric_status; + + CapabilitiesFabric(const rclcpp::NodeOptions& options = rclcpp::NodeOptions()) : Node("Capabilities2_Fabric", options), lock_(mutex_, std::defer_lock) { control_tag_list = xml_parser::get_control_list(); } @@ -75,6 +82,10 @@ class CapabilitiesFabric : public rclcpp::Node std::bind(&CapabilitiesFabric::handle_cancel, this, std::placeholders::_1), std::bind(&CapabilitiesFabric::handle_accepted, this, std::placeholders::_1)); + completion_server_ = + this->create_service("/capabilities_fabric/set_completion", + std::bind(&CapabilitiesFabric::setCompleteCallback, this, std::placeholders::_1, std::placeholders::_2)); + get_interfaces_client_ = this->create_client("/capabilities/get_interfaces"); get_sem_interf_client_ = this->create_client("/capabilities/get_semantic_interfaces"); get_providers_client_ = this->create_client("/capabilities/get_providers"); @@ -96,6 +107,8 @@ class CapabilitiesFabric : public rclcpp::Node feedback_msg = std::make_shared(); result_msg = std::make_shared(); + + fabric_state = Status::IDLE; } private: @@ -114,7 +127,7 @@ class CapabilitiesFabric : public rclcpp::Node (void)uuid; // try to parse the std::string plan from capabilities_msgs/Plan to the to a XMLDocument file - tinyxml2::XMLError xml_status = document.Parse(goal->plan.c_str()); + tinyxml2::XMLError xml_status = documentEvaluation.Parse(goal->plan.c_str()); // check if the file parsing failed if (xml_status != tinyxml2::XMLError::XML_SUCCESS) @@ -123,9 +136,18 @@ class CapabilitiesFabric : public rclcpp::Node return rclcpp_action::GoalResponse::REJECT; } - status_->info("Plan parsed and accepted"); - - return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + if (fabric_state == Status::RUNNING) + { + status_->info("Prior plan under exeution. Will defer the new plan"); + plan_queue.push_back(goal->plan); + return rclcpp_action::GoalResponse::ACCEPT_AND_DEFER; + } + else + { + status_->info("Plan parsed and accepted"); + plan_queue.push_back(goal->plan); + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + } } /** @@ -141,6 +163,8 @@ class CapabilitiesFabric : public rclcpp::Node bond_client_->stop(); + fabric_state = Status::CANCELED; + return rclcpp_action::CancelResponse::ACCEPT; } @@ -153,15 +177,26 @@ class CapabilitiesFabric : public rclcpp::Node { goal_handle_ = goal_handle; + fabric_state = Status::RUNNING; + execution(); } + void setCompleteCallback(const std::shared_ptr request, std::shared_ptr response) + { + fabric_completed_ = true; + cv_.notify_all(); + } + /** * @brief Start the execution of the capabilities2 fabric */ void execution() { - process_feedback("Execution started"); + process_feedback("A new execution started"); + + lock_.lock(); + fabric_completed_ = false; expected_providers_ = 0; completed_providers_ = 0; @@ -175,7 +210,22 @@ class CapabilitiesFabric : public rclcpp::Node expected_configurations_ = 0; completed_configurations_ = 0; - getInterfaces(); + std::string plan_to_be_executed = plan_queue[0]; + + tinyxml2::XMLError xml_status = document.Parse(plan_to_be_executed.c_str()); + + // check if the file parsing failed + if (xml_status != tinyxml2::XMLError::XML_SUCCESS) + { + status_->error("Parsing the plan from queue failed"); + return; + } + else + { + status_->error("Parsing the plan from queue successful"); + plan_queue.pop_front(); + getInterfaces(); + } } /** @@ -200,8 +250,7 @@ class CapabilitiesFabric : public rclcpp::Node auto response = future.get(); expected_interfaces_ = response->interfaces.size(); - status = "Received Interfaces. Requsting " + std::to_string(expected_interfaces_) + " semantic interface information"; - process_feedback(status); + process_feedback("Received Interfaces. Requsting " + std::to_string(expected_interfaces_) + " semantic interface information"); // Request each interface recursively for Semantic interfaces getSemanticInterfaces(response->interfaces); @@ -217,8 +266,7 @@ class CapabilitiesFabric : public rclcpp::Node { std::string requested_interface = interfaces[completed_interfaces_]; - status = "Requesting semantic interfaces for " + requested_interface; - process_feedback(status, true); + process_feedback("Requesting semantic interfaces for " + requested_interface, true); auto request_semantic = std::make_shared(); request_semantic->interface = requested_interface; @@ -243,9 +291,8 @@ class CapabilitiesFabric : public rclcpp::Node interface_list.push_back(semantic_interface); is_semantic_list.push_back(true); - status = (completed_interfaces_) + "/" + std::to_string(expected_interfaces_) + " : Received " + semantic_interface + " for " + - requested_interface + ". So added " + semantic_interface; - process_feedback(status); + process_feedback(std::to_string(completed_interfaces_) + "/" + std::to_string(expected_interfaces_) + " : Received " + + semantic_interface + " for " + requested_interface + ". So added " + semantic_interface); } } // if no semantic interfaces are availble for a given interface, add the interface instead @@ -254,9 +301,8 @@ class CapabilitiesFabric : public rclcpp::Node interface_list.push_back(requested_interface); is_semantic_list.push_back(false); - status = std::to_string(completed_interfaces_) + "/" + std::to_string(expected_interfaces_) + " : Received none for " + - requested_interface + ". So added " + requested_interface; - process_feedback(status); + process_feedback(std::to_string(completed_interfaces_) + "/" + std::to_string(expected_interfaces_) + " : Received none for " + + requested_interface + ". So added " + requested_interface); } if (completed_interfaces_ != expected_interfaces_) @@ -270,8 +316,7 @@ class CapabilitiesFabric : public rclcpp::Node expected_providers_ = interface_list.size(); - status = "Requsting Provider information for " + std::to_string(expected_providers_) + " providers"; - process_feedback(status); + process_feedback("Requsting Provider information for " + std::to_string(expected_providers_) + " providers"); // request providers from the interfaces in the interfaces_list getProvider(interface_list, is_semantic_list); @@ -290,8 +335,7 @@ class CapabilitiesFabric : public rclcpp::Node std::string requested_interface = interfaces[completed_providers_]; bool semantic_flag = is_semantic[completed_providers_]; - status = "Requesting provider for " + requested_interface; - process_feedback(status, true); + process_feedback("Requesting provider for " + requested_interface, true); auto request_providers = std::make_shared(); @@ -303,8 +347,7 @@ class CapabilitiesFabric : public rclcpp::Node request_providers, [this, is_semantic, requested_interface, interfaces](GetProvidersClient::SharedFuture future) { if (!future.valid()) { - status = "Did not retrieve providers for interface: " + requested_interface; - process_result(status, false, false); + process_result("Did not retrieve providers for interface: " + requested_interface, false, false); return; } @@ -316,9 +359,8 @@ class CapabilitiesFabric : public rclcpp::Node // add defualt provider to the list providers_list.push_back(response->default_provider); - status = std::to_string(completed_providers_) + "/" + std::to_string(expected_providers_) + " : Received " + response->default_provider + - " for " + requested_interface + ". So added " + response->default_provider; - process_feedback(status); + process_feedback(std::to_string(completed_providers_) + "/" + std::to_string(expected_providers_) + " : Received " + + response->default_provider + " for " + requested_interface + ". So added " + response->default_provider); } // add additional providers to the list if available @@ -328,15 +370,14 @@ class CapabilitiesFabric : public rclcpp::Node { providers_list.push_back(provider); - status = std::to_string(completed_providers_) + "/" + std::to_string(expected_providers_) + " : Received and added " + provider + - " for " + requested_interface; - process_feedback(status); + process_feedback(std::to_string(completed_providers_) + "/" + std::to_string(expected_providers_) + " : Received and added " + + provider + " for " + requested_interface); } } else { - status = std::to_string(completed_providers_) + "/" + std::to_string(expected_providers_) + " : No providers for " + requested_interface; - process_feedback(status); + process_feedback(std::to_string(completed_providers_) + "/" + std::to_string(expected_providers_) + " : No providers for " + + requested_interface); } // Check if all expected calls are completed before calling verify_plan @@ -469,16 +510,14 @@ class CapabilitiesFabric : public rclcpp::Node auto response = future.get(); bond_id_ = response->bond_id; - status = "Received the bond id : " + bond_id_; - process_feedback(status); + process_feedback("Received the bond id : " + bond_id_); bond_client_ = std::make_unique(shared_from_this(), bond_id_); bond_client_->start(); expected_capabilities_ = connection_map.size(); - status = "Requsting start of " + std::to_string(expected_capabilities_) + " capabilities"; - process_feedback(status); + process_feedback("Requsting start of " + std::to_string(expected_capabilities_) + " capabilities"); use_capability(connection_map); }); @@ -500,28 +539,25 @@ class CapabilitiesFabric : public rclcpp::Node request_use->preferred_provider = provider; request_use->bond_id = bond_id_; - status = "Starting capability of Runner " + std::to_string(completed_capabilities_) + " : " + capabilities[completed_capabilities_].source.runner; - process_feedback(status, true); + process_feedback("Starting capability of Runner " + std::to_string(completed_capabilities_) + " : " + + capabilities[completed_capabilities_].source.runner, + true); // send the request auto result_future = use_capability_client_->async_send_request(request_use, [this, capability, provider](UseCapabilityClient::SharedFuture future) { if (!future.valid()) { - status = "Failed to Use capability " + capability + " from " + provider + ". Server Execution Cancelled"; - process_result(status); + process_result("Failed to Use capability " + capability + " from " + provider + ". Server Execution Cancelled"); // release all capabilities that were used since not all started successfully for (const auto& [key, value] : connection_map) { - status = "Freeing capability of Node " + std::to_string(key) + " named " + value.source.runner; - process_feedback(status); - + process_feedback("Freeing capability of Node " + std::to_string(key) + " named " + value.source.runner); free_capability(value.source.runner); } bond_client_->stop(); - return; } @@ -529,8 +565,7 @@ class CapabilitiesFabric : public rclcpp::Node auto response = future.get(); - status = std::to_string(completed_capabilities_) + "/" + std::to_string(expected_capabilities_) + " : start succeessful"; - process_feedback(status); + process_feedback(std::to_string(completed_capabilities_) + "/" + std::to_string(expected_capabilities_) + " : start succeessful"); // Check if all expected calls are completed before calling verify_plan if (completed_capabilities_ == expected_capabilities_) @@ -539,8 +574,7 @@ class CapabilitiesFabric : public rclcpp::Node expected_configurations_ = connection_map.size(); - status = "Requsting capability configuration for " + std::to_string(expected_configurations_) + " capabilities"; - process_feedback(status, true); + process_feedback("Requsting capability configuration for " + std::to_string(expected_configurations_) + " capabilities", true); configure_capabilities(connection_map); } @@ -566,15 +600,12 @@ class CapabilitiesFabric : public rclcpp::Node auto result_future = free_capability_client_->async_send_request(request_free, [this, capability](FreeCapabilityClient::SharedFuture future) { if (!future.valid()) { - status = "Failed to free capability " + capability; - process_result(status); + process_result("Failed to free capability " + capability); return; } auto response = future.get(); - - status = "Successfully freed capability " + capability; - process_feedback(status, true); + process_feedback("Successfully freed capability " + capability, true); }); } @@ -585,9 +616,9 @@ class CapabilitiesFabric : public rclcpp::Node { auto request_configure = std::make_shared(); - status = "Configuring capability of Runner " + std::to_string(completed_configurations_) + " named " + - capabilities[completed_configurations_].source.runner; - process_feedback(status, true); + process_feedback("Configuring capability of Runner " + std::to_string(completed_configurations_) + " named " + + capabilities[completed_configurations_].source.runner, + true); if (xml_parser::convert_to_string(capabilities[completed_configurations_].source.parameters, request_configure->source.parameters)) { @@ -608,8 +639,8 @@ class CapabilitiesFabric : public rclcpp::Node } else { - request_configure->target_on_start.capability = ""; - request_configure->target_on_start.provider = ""; + request_configure->target_on_start.capability = "std_capabilities/FabricCompletionRunner"; + request_configure->target_on_start.provider = "std_capabilities/FabricCompletionRunner"; } if (xml_parser::convert_to_string(capabilities[completed_configurations_].target_on_stop.parameters, @@ -620,8 +651,8 @@ class CapabilitiesFabric : public rclcpp::Node } else { - request_configure->target_on_stop.capability = ""; - request_configure->target_on_stop.provider = ""; + request_configure->target_on_stop.capability = "std_capabilities/FabricCompletionRunner"; + request_configure->target_on_stop.provider = "std_capabilities/FabricCompletionRunner"; } if (xml_parser::convert_to_string(capabilities[completed_configurations_].target_on_success.parameters, @@ -632,8 +663,8 @@ class CapabilitiesFabric : public rclcpp::Node } else { - request_configure->target_on_success.capability = ""; - request_configure->target_on_success.provider = ""; + request_configure->target_on_success.capability = "std_capabilities/FabricCompletionRunner"; + request_configure->target_on_success.provider = "std_capabilities/FabricCompletionRunner"; } if (xml_parser::convert_to_string(capabilities[completed_configurations_].target_on_failure.parameters, @@ -644,8 +675,8 @@ class CapabilitiesFabric : public rclcpp::Node } else { - request_configure->target_on_failure.capability = ""; - request_configure->target_on_failure.provider = ""; + request_configure->target_on_failure.capability = "std_capabilities/FabricCompletionRunner"; + request_configure->target_on_failure.provider = "std_capabilities/FabricCompletionRunner"; } std::string source_capability = capabilities[completed_configurations_].source.runner; @@ -655,8 +686,7 @@ class CapabilitiesFabric : public rclcpp::Node conf_capability_client_->async_send_request(request_configure, [this, source_capability](ConfigureCapabilityClient::SharedFuture future) { if (!future.valid()) { - status = "Failed to configure capability :" + source_capability + ". Server execution cancelled"; - process_result(status); + process_result("Failed to configure capability :" + source_capability + ". Server execution cancelled"); return; } @@ -664,9 +694,8 @@ class CapabilitiesFabric : public rclcpp::Node auto response = future.get(); - status = std::to_string(completed_configurations_) + "/" + std::to_string(expected_configurations_) + - " : Successfully configured capability : " + source_capability; - process_feedback(status); + process_feedback(std::to_string(completed_configurations_) + "/" + std::to_string(expected_configurations_) + + " : Successfully configured capability : " + source_capability); // Check if all expected calls are completed before calling verify_plan if (completed_configurations_ == expected_configurations_) @@ -698,32 +727,55 @@ class CapabilitiesFabric : public rclcpp::Node auto result_future = trig_capability_client_->async_send_request(request_trigger, [this](TriggerCapabilityClient::SharedFuture future) { if (!future.valid()) { - status = "Failed to trigger capability " + connection_map[0].source.runner; - process_result(status); + process_result("Failed to trigger capability " + connection_map[0].source.runner); return; } auto response = future.get(); + process_feedback("Successfully triggered capability " + connection_map[0].source.runner); - status = "Successfully triggered capability " + connection_map[0].source.runner; - process_feedback(status); - - process_result("Successfully launched capabilities2 fabric", true); + wait_for_completion(); }); } + /** + * @brief Wait for the execution completion of the fabric + * + */ + void wait_for_completion() + { + process_feedback("Waiting for fabric execution completion"); + + // Conditional wait + while (!fabric_completed_) + { + cv_.wait(lock_); + } + + lock_.unlock(); + + if (plan_queue.size()>0) + { + process_feedback("Successfully completed capabilities2 fabric. Starting the next fabric"); + + execution(); + } + else + { + process_result("Successfully completed capabilities2 fabric", true); + } + } + void check_service(bool wait_for_logic, const std::string& service_name) { while (wait_for_logic) { - std::string failed = service_name + " not available"; - status_->error(failed); + status_->error(service_name + " not available"); rclcpp::shutdown(); return; } - std::string success = service_name + " connected"; - status_->info(success); + status_->info(service_name + " connected"); } /** @@ -754,13 +806,15 @@ class CapabilitiesFabric : public rclcpp::Node if (success) { - goal_handle_->succeed(result_msg); status_->info(text, newline); + fabric_state = Status::SUCCEEDED; + goal_handle_->succeed(result_msg); } else { - goal_handle_->abort(result_msg); status_->error(text, newline); + fabric_state = Status::ABORTED; + goal_handle_->abort(result_msg); } } @@ -783,21 +837,27 @@ class CapabilitiesFabric : public rclcpp::Node int expected_configurations_; int completed_configurations_; - /** status message string */ - std::string status; - /** Bond id */ std::string bond_id_; + /** Status of the fabric */ + Status fabric_state; + /** Manages bond between capabilities server and this client */ std::shared_ptr bond_client_; /** Handles status message sending and printing to logging */ std::shared_ptr status_; + /** Vector of plans */ + std::deque plan_queue; + /** XML Document */ tinyxml2::XMLDocument document; + /** XML Document */ + tinyxml2::XMLDocument documentEvaluation; + /** vector of connections */ std::map connection_map; @@ -851,4 +911,13 @@ class CapabilitiesFabric : public rclcpp::Node /** trigger an selected capability */ TriggerCapabilityClient::SharedPtr trig_capability_client_; + + /** server to get the status of the capabilities2 fabric */ + rclcpp::Service::SharedPtr completion_server_; + + /** capabilities2 server and fabric synchronization tools */ + std::mutex mutex_; + std::condition_variable cv_; + bool fabric_completed_; + std::unique_lock lock_; }; \ No newline at end of file diff --git a/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric_client.hpp b/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric_client.hpp index a13976c..0b5670a 100644 --- a/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric_client.hpp +++ b/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric_client.hpp @@ -12,6 +12,7 @@ #include #include +#include #include @@ -29,16 +30,8 @@ class CapabilitiesFabricClient : public rclcpp::Node { public: - enum fabric_status - { - IDLE, - RUNNING, - CANCELED, - ABORTED, - FAILED, - SUCCEEDED - }; + using Status = capabilities2::fabric_status; using Plan = capabilities2_msgs::action::Plan; using GoalHandlePlan = rclcpp_action::ClientGoalHandle; @@ -58,7 +51,7 @@ class CapabilitiesFabricClient : public rclcpp::Node */ void initialize() { - fabric_state = fabric_status::IDLE; + fabric_state = Status::IDLE; status_ = std::make_shared(shared_from_this(), "capabilities_fabric_client", "/status/capabilities_fabric_client"); @@ -91,13 +84,11 @@ class CapabilitiesFabricClient : public rclcpp::Node // check if the file loading failed if (xml_status != tinyxml2::XMLError::XML_SUCCESS) { - status = "Error loading plan: " + plan_file_path + ", Error: " + document.ErrorName(); - status_->error(status); + status_->error("Error loading plan: " + plan_file_path + ", Error: " + document.ErrorName()); rclcpp::shutdown(); } - status = "Plan loaded from : " + plan_file_path; - status_->info(status); + status_->info("Plan loaded from : " + plan_file_path); send_goal(document); } @@ -109,9 +100,7 @@ class CapabilitiesFabricClient : public rclcpp::Node xml_parser::convert_to_string(document_xml, goal_msg.plan); - status = "Following plan was loaded :\n\n " + goal_msg.plan; - status_->info(status); - + status_->info("Following plan was loaded :\n\n " + goal_msg.plan); status_->info("Sending goal to the capabilities_fabric action server"); auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); @@ -122,13 +111,13 @@ class CapabilitiesFabricClient : public rclcpp::Node if (!goal_handle) { status_->error("Goal was rejected by server"); - fabric_state = fabric_status::FAILED; + fabric_state = Status::FAILED; } else { status_->info("Goal accepted by server, waiting for result"); goal_handle_ = goal_handle; - fabric_state = fabric_status::RUNNING; + fabric_state = Status::RUNNING; } }; @@ -137,19 +126,19 @@ class CapabilitiesFabricClient : public rclcpp::Node switch (result.code) { case rclcpp_action::ResultCode::SUCCEEDED: - fabric_state = fabric_status::SUCCEEDED; + fabric_state = Status::SUCCEEDED; break; case rclcpp_action::ResultCode::ABORTED: status_->error("Goal was aborted"); - fabric_state = fabric_status::ABORTED; + fabric_state = Status::ABORTED; break; case rclcpp_action::ResultCode::CANCELED: status_->error("Goal was canceled"); - fabric_state = fabric_status::CANCELED; + fabric_state = Status::CANCELED; break; default: status_->error("Unknown result code"); - fabric_state = fabric_status::FAILED; + fabric_state = Status::FAILED; break; } @@ -176,27 +165,27 @@ class CapabilitiesFabricClient : public rclcpp::Node void getStatusCallback(const std::shared_ptr request, std::shared_ptr response) { - if (fabric_state == fabric_status::IDLE) + if (fabric_state == Status::IDLE) { response->status = GetFabricStatus::Response::FABRIC_IDLE; } - else if (fabric_state == fabric_status::RUNNING) + else if (fabric_state == Status::RUNNING) { response->status = GetFabricStatus::Response::FABRIC_RUNNING; } - else if (fabric_state == fabric_status::CANCELED) + else if (fabric_state == Status::CANCELED) { response->status = GetFabricStatus::Response::FABRIC_CANCELED; } - else if (fabric_state == fabric_status::ABORTED) + else if (fabric_state == Status::ABORTED) { response->status = GetFabricStatus::Response::FABRIC_ABORTED; } - else if (fabric_state == fabric_status::FAILED) + else if (fabric_state == Status::FAILED) { response->status = GetFabricStatus::Response::FABRIC_FAILED; } - else if (fabric_state == fabric_status::SUCCEEDED) + else if (fabric_state == Status::SUCCEEDED) { response->status = GetFabricStatus::Response::FABRIC_SUCCEEDED; } @@ -229,7 +218,7 @@ class CapabilitiesFabricClient : public rclcpp::Node void cancelPlanCallback(const std::shared_ptr request, std::shared_ptr response) { - if (fabric_state == fabric_status::RUNNING) + if (fabric_state == Status::RUNNING) { this->planner_client_->async_cancel_goal(goal_handle_); } @@ -266,5 +255,5 @@ class CapabilitiesFabricClient : public rclcpp::Node rclcpp::Service::SharedPtr cancel_server_; /** Status of the fabric */ - fabric_status fabric_state; + Status fabric_state; }; diff --git a/capabilities2_fabric/include/capabilities2_fabric/utils/fabric_status.hpp b/capabilities2_fabric/include/capabilities2_fabric/utils/fabric_status.hpp new file mode 100644 index 0000000..bd3fffb --- /dev/null +++ b/capabilities2_fabric/include/capabilities2_fabric/utils/fabric_status.hpp @@ -0,0 +1,14 @@ +#pragma once + +namespace capabilities2 +{ +enum fabric_status +{ + IDLE, + RUNNING, + CANCELED, + ABORTED, + FAILED, + SUCCEEDED +}; +} \ No newline at end of file diff --git a/capabilities2_msgs/CMakeLists.txt b/capabilities2_msgs/CMakeLists.txt index b50c9eb..05e02d5 100644 --- a/capabilities2_msgs/CMakeLists.txt +++ b/capabilities2_msgs/CMakeLists.txt @@ -47,6 +47,7 @@ set(srv_files "srv/GetFabricStatus.srv" "srv/SetFabricPlan.srv" "srv/CancelFabricPlan.srv" + "srv/CompleteFabric.srv" ) set(action_files diff --git a/capabilities2_msgs/srv/CompleteFabric.srv b/capabilities2_msgs/srv/CompleteFabric.srv new file mode 100644 index 0000000..73b314f --- /dev/null +++ b/capabilities2_msgs/srv/CompleteFabric.srv @@ -0,0 +1 @@ +--- \ No newline at end of file diff --git a/capabilities2_runner/include/capabilities2_runner/action_runner.hpp b/capabilities2_runner/include/capabilities2_runner/action_runner.hpp index 4ca876e..43f0fde 100644 --- a/capabilities2_runner/include/capabilities2_runner/action_runner.hpp +++ b/capabilities2_runner/include/capabilities2_runner/action_runner.hpp @@ -137,8 +137,8 @@ class ActionRunner : public RunnerBase info_("goal generated", id); - std::unique_lock lock(send_goal_mutex); - action_complete = false; + std::unique_lock lock(mutex_); + completed_ = false; // trigger the action client with goal send_goal_options_.goal_response_callback = @@ -201,15 +201,15 @@ class ActionRunner : public RunnerBase } result_ = wrapped_result.result; - action_complete = true; - send_goal_cv.notify_all(); + completed_ = true; + cv_.notify_all(); }; goal_handle_future_ = action_client_->async_send_goal(goal_msg_, send_goal_options_); info_("goal sent. Waiting for acceptance.", id); // Conditional wait - send_goal_cv.wait(lock, [this] { return action_complete; }); + cv_.wait(lock, [this] { return completed_; }); info_("action complete. Thread closing.", id); } @@ -264,11 +264,6 @@ class ActionRunner : public RunnerBase /** Result Future*/ std::shared_future::WrappedResult> result_future_; - - // Synchronization tools - std::mutex mutex_; - std::condition_variable cv_; - bool action_completed_; }; } // namespace capabilities2_runner diff --git a/capabilities2_runner/include/capabilities2_runner/runner_base.hpp b/capabilities2_runner/include/capabilities2_runner/runner_base.hpp index 46249b3..11e4ec2 100644 --- a/capabilities2_runner/include/capabilities2_runner/runner_base.hpp +++ b/capabilities2_runner/include/capabilities2_runner/runner_base.hpp @@ -570,15 +570,19 @@ class RunnerBase std::map executionThreadPool; /** - * @brief mutex and conditional variable for threadpool synchronisation. + * @brief mutex for threadpool synchronisation. */ - std::mutex send_goal_mutex; - std::condition_variable send_goal_cv; + std::mutex mutex_; /** - * @brief boolean flag for thread completion. + * @brief conditional variable for threadpool synchronisation. */ - bool action_complete; + std::condition_variable cv_; + + /** + * @brief flag for threadpool synchronisation. + */ + bool completed_; /** * @brief external function that triggers capability runners diff --git a/capabilities2_runner/include/capabilities2_runner/service_runner.hpp b/capabilities2_runner/include/capabilities2_runner/service_runner.hpp index 46040a8..eda0065 100644 --- a/capabilities2_runner/include/capabilities2_runner/service_runner.hpp +++ b/capabilities2_runner/include/capabilities2_runner/service_runner.hpp @@ -72,8 +72,8 @@ class ServiceRunner : public RunnerBase info_("request generated", id); - std::unique_lock lock(send_goal_mutex); - action_complete = false; + std::unique_lock lock(mutex_); + completed_ = false; auto result_future = service_client_->async_send_request( request_msg, [this, id](typename rclcpp::Client::SharedFuture future) { @@ -102,8 +102,8 @@ class ServiceRunner : public RunnerBase } } - action_complete = true; - send_goal_cv.notify_all(); + completed_ = true; + cv_.notify_all(); }); // trigger the events related to on_started state @@ -114,7 +114,7 @@ class ServiceRunner : public RunnerBase } // Conditional wait - send_goal_cv.wait(lock, [this] { return action_complete; }); + cv_.wait(lock, [this] { return completed_; }); info_("Service request complete. Thread closing.", id); } diff --git a/capabilities2_runner_capabilities/.clang-format b/capabilities2_runner_capabilities/.clang-format new file mode 100644 index 0000000..d36804f --- /dev/null +++ b/capabilities2_runner_capabilities/.clang-format @@ -0,0 +1,64 @@ + +BasedOnStyle: Google +AccessModifierOffset: -2 +ConstructorInitializerIndentWidth: 2 +AlignEscapedNewlinesLeft: false +AlignTrailingComments: true +AllowAllParametersOfDeclarationOnNextLine: false +AllowShortIfStatementsOnASingleLine: false +AllowShortLoopsOnASingleLine: false +AllowShortFunctionsOnASingleLine: None +AlwaysBreakTemplateDeclarations: true +AlwaysBreakBeforeMultilineStrings: false +BreakBeforeBinaryOperators: false +BreakBeforeTernaryOperators: false +BreakConstructorInitializersBeforeComma: true +BinPackParameters: true +ColumnLimit: 120 +ConstructorInitializerAllOnOneLineOrOnePerLine: true +DerivePointerBinding: false +PointerBindsToType: true +ExperimentalAutoDetectBinPacking: false +IndentCaseLabels: true +MaxEmptyLinesToKeep: 1 +NamespaceIndentation: None +ObjCSpaceBeforeProtocolList: true +PenaltyBreakBeforeFirstCallParameter: 19 +PenaltyBreakComment: 60 +PenaltyBreakString: 1 +PenaltyBreakFirstLessLess: 1000 +PenaltyExcessCharacter: 1000 +PenaltyReturnTypeOnItsOwnLine: 90 +SpacesBeforeTrailingComments: 2 +Cpp11BracedListStyle: false +Standard: Auto +IndentWidth: 2 +TabWidth: 2 +UseTab: Never +IndentFunctionDeclarationAfterType: false +SpacesInParentheses: false +SpacesInAngles: false +SpaceInEmptyParentheses: false +SpacesInCStyleCastParentheses: false +SpaceAfterControlStatementKeyword: true +SpaceBeforeAssignmentOperators: true +ContinuationIndentWidth: 4 +SortIncludes: false +SpaceAfterCStyleCast: false + +# Configure each individual brace in BraceWrapping +BreakBeforeBraces: Custom + +# Control of individual brace wrapping cases +BraceWrapping: { + AfterClass: 'true' + AfterControlStatement: 'true' + AfterEnum : 'true' + AfterFunction : 'true' + AfterNamespace : 'true' + AfterStruct : 'true' + AfterUnion : 'true' + BeforeCatch : 'true' + BeforeElse : 'true' + IndentBraces : 'false' +} diff --git a/capabilities2_runner_capabilities/CMakeLists.txt b/capabilities2_runner_capabilities/CMakeLists.txt new file mode 100644 index 0000000..d72bd90 --- /dev/null +++ b/capabilities2_runner_capabilities/CMakeLists.txt @@ -0,0 +1,63 @@ +cmake_minimum_required(VERSION 3.8) +project(capabilities2_runner_capabilities) + +# Default to C++17 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_action REQUIRED) +find_package(pluginlib REQUIRED) +find_package(capabilities2_msgs REQUIRED) +find_package(capabilities2_runner REQUIRED) + +# Locate the static version of tinyxml2 +find_library(TINYXML2_LIB NAMES tinyxml2 PATHS /usr/local/lib NO_DEFAULT_PATH) + +if(NOT TINYXML2_LIB) + message(FATAL_ERROR "tinyxml2 library not found. Make sure it's installed as a static library.") +endif() + +include_directories(include + /usr/local/include +) + +add_library(${PROJECT_NAME} SHARED + src/capabilities2_runner.cpp +) + +target_link_libraries(${PROJECT_NAME} + ${TINYXML2_LIB} +) + +ament_target_dependencies(${PROJECT_NAME} + rclcpp + rclcpp_action + pluginlib + capabilities2_msgs + capabilities2_runner +) + +pluginlib_export_plugin_description_file(capabilities2_runner plugins.xml) + +install(TARGETS ${PROJECT_NAME} + EXPORT ${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +install(DIRECTORY include/ + DESTINATION include +) + +ament_export_include_directories(include) +ament_export_libraries(${PROJECT_NAME}) + +ament_package() diff --git a/capabilities2_runner_capabilities/include/capabilities2_runner_capabilities/fabric_completion_runner.hpp b/capabilities2_runner_capabilities/include/capabilities2_runner_capabilities/fabric_completion_runner.hpp new file mode 100644 index 0000000..828fb7c --- /dev/null +++ b/capabilities2_runner_capabilities/include/capabilities2_runner_capabilities/fabric_completion_runner.hpp @@ -0,0 +1,53 @@ +#pragma once +#include +#include +#include +#include +#include + +namespace capabilities2_runner +{ +/** + * @brief fabric completion runner + * + * This class is a wrapper around the capabilities2 service runner and is used to + * call on the /capabilities_fabric/set_completion service, providing it as a + * capability that notifys the completion of the fabric + */ +class FabricCompletionRunner : public ServiceRunner +{ +public: + FabricCompletionRunner() : ServiceRunner() + { + } + + /** + * @brief Starter function for starting the service runner + * + * @param node shared pointer to the capabilities node. Allows to use ros node related functionalities + * @param run_config runner configuration loaded from the yaml file + */ + virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config, + std::function print) override + { + init_service(node, run_config, "/capabilities_fabric/set_completion", print); + } + +protected: + /** + * @brief Generate a empty request. + * + * This function is used in conjunction with the trigger function to inject type erased parameters + * into the typed action + * + * @param parameters + * @return prompt_msgs::srv::Prompt::Request the generated request + */ + virtual typename capabilities2_msgs::srv::CompleteFabric::Request generate_request(tinyxml2::XMLElement* parameters, int id) override + { + capabilities2_msgs::srv::CompleteFabric::Request request; + return request; + } +}; + +} // namespace capabilities2_runner diff --git a/capabilities2_runner_capabilities/package.xml b/capabilities2_runner_capabilities/package.xml new file mode 100644 index 0000000..93ddac1 --- /dev/null +++ b/capabilities2_runner_capabilities/package.xml @@ -0,0 +1,28 @@ + + + + capabilities2_runner_capabilities + 0.0.0 + TODO: Package description + kalana + TODO: License declaration + + ament_cmake + + ament_lint_auto + ament_lint_common + + rclcpp + rclcpp_action + pluginlib + std_msgs + capabilities2_msgs + capabilities2_runner + + + tinyxml2 + + + ament_cmake + + diff --git a/capabilities2_runner_capabilities/plugins.xml b/capabilities2_runner_capabilities/plugins.xml new file mode 100644 index 0000000..4789f11 --- /dev/null +++ b/capabilities2_runner_capabilities/plugins.xml @@ -0,0 +1,7 @@ + + + + A plugin that notifies about the completion of the fabric to the action server + + + diff --git a/capabilities2_runner_capabilities/src/capabilities2_runner.cpp b/capabilities2_runner_capabilities/src/capabilities2_runner.cpp new file mode 100644 index 0000000..26e1b95 --- /dev/null +++ b/capabilities2_runner_capabilities/src/capabilities2_runner.cpp @@ -0,0 +1,6 @@ +#include +#include +#include + +// register runner plugins +PLUGINLIB_EXPORT_CLASS(capabilities2_runner::FabricCompletionRunner, capabilities2_runner::RunnerBase) \ No newline at end of file diff --git a/capabilities2_runner_nav2/CMakeLists.txt b/capabilities2_runner_nav2/CMakeLists.txt index 1ce3c58..3edafbc 100644 --- a/capabilities2_runner_nav2/CMakeLists.txt +++ b/capabilities2_runner_nav2/CMakeLists.txt @@ -16,7 +16,6 @@ find_package(rclcpp_action REQUIRED) find_package(pluginlib REQUIRED) find_package(nav2_msgs REQUIRED) find_package(geometry_msgs REQUIRED) -find_package(lifecycle_msgs REQUIRED) find_package(capabilities2_msgs REQUIRED) find_package(capabilities2_runner REQUIRED) @@ -45,7 +44,6 @@ ament_target_dependencies(${PROJECT_NAME} pluginlib nav2_msgs geometry_msgs - lifecycle_msgs capabilities2_msgs capabilities2_runner ) 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 dc7a14d..1a93591 100644 --- a/capabilities2_runner_nav2/include/capabilities2_runner_nav2/waypoint_runner.hpp +++ b/capabilities2_runner_nav2/include/capabilities2_runner_nav2/waypoint_runner.hpp @@ -6,7 +6,6 @@ #include #include #include -#include #include #include @@ -94,8 +93,6 @@ class WayPointRunner : public ActionRunner std::string robot_base_frame_; /**The frame of the robot base*/ double x, y; /**Coordinate frame parameters*/ - - rclcpp::Client::SharedPtr get_state_client_; }; } // namespace capabilities2_runner From d5882fbef7c79145186ed1352cf0040c40c2f44a Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Tue, 4 Feb 2025 17:47:34 +1100 Subject: [PATCH 083/133] restored fabric to original and modified fabric client with completion server and threading for sending goal --- .../capabilities_fabric.hpp | 132 +++--------------- .../capabilities_fabric_client.hpp | 119 +++++++++++++--- .../utils/fabric_status.hpp | 3 +- .../capabilities2_fabric/utils/xml_parser.hpp | 34 +++++ capabilities2_msgs/srv/GetFabricStatus.srv | 5 +- 5 files changed, 160 insertions(+), 133 deletions(-) diff --git a/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric.hpp b/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric.hpp index d2cb291..454a698 100644 --- a/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric.hpp +++ b/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric.hpp @@ -2,7 +2,6 @@ #include #include #include -#include #include #include @@ -12,7 +11,6 @@ #include #include #include -#include #include @@ -24,7 +22,6 @@ #include #include #include -#include /** * @brief Capabilities Fabric @@ -49,8 +46,6 @@ class CapabilitiesFabric : public rclcpp::Node using ConfigureCapability = capabilities2_msgs::srv::ConfigureCapability; using TriggerCapability = capabilities2_msgs::srv::TriggerCapability; - using CompleteFabric = capabilities2_msgs::srv::CompleteFabric; - using GetInterfacesClient = rclcpp::Client; using GetSemanticInterfacesClient = rclcpp::Client; using GetProvidersClient = rclcpp::Client; @@ -60,9 +55,7 @@ class CapabilitiesFabric : public rclcpp::Node using ConfigureCapabilityClient = rclcpp::Client; using TriggerCapabilityClient = rclcpp::Client; - using Status = capabilities2::fabric_status; - - CapabilitiesFabric(const rclcpp::NodeOptions& options = rclcpp::NodeOptions()) : Node("Capabilities2_Fabric", options), lock_(mutex_, std::defer_lock) + CapabilitiesFabric(const rclcpp::NodeOptions& options = rclcpp::NodeOptions()) : Node("Capabilities2_Fabric", options) { control_tag_list = xml_parser::get_control_list(); } @@ -82,10 +75,6 @@ class CapabilitiesFabric : public rclcpp::Node std::bind(&CapabilitiesFabric::handle_cancel, this, std::placeholders::_1), std::bind(&CapabilitiesFabric::handle_accepted, this, std::placeholders::_1)); - completion_server_ = - this->create_service("/capabilities_fabric/set_completion", - std::bind(&CapabilitiesFabric::setCompleteCallback, this, std::placeholders::_1, std::placeholders::_2)); - get_interfaces_client_ = this->create_client("/capabilities/get_interfaces"); get_sem_interf_client_ = this->create_client("/capabilities/get_semantic_interfaces"); get_providers_client_ = this->create_client("/capabilities/get_providers"); @@ -107,8 +96,6 @@ class CapabilitiesFabric : public rclcpp::Node feedback_msg = std::make_shared(); result_msg = std::make_shared(); - - fabric_state = Status::IDLE; } private: @@ -126,8 +113,10 @@ class CapabilitiesFabric : public rclcpp::Node (void)uuid; + status_->info("Following plan was received :\n\n " + goal->plan); + // try to parse the std::string plan from capabilities_msgs/Plan to the to a XMLDocument file - tinyxml2::XMLError xml_status = documentEvaluation.Parse(goal->plan.c_str()); + tinyxml2::XMLError xml_status = document.Parse(goal->plan.c_str()); // check if the file parsing failed if (xml_status != tinyxml2::XMLError::XML_SUCCESS) @@ -136,18 +125,9 @@ class CapabilitiesFabric : public rclcpp::Node return rclcpp_action::GoalResponse::REJECT; } - if (fabric_state == Status::RUNNING) - { - status_->info("Prior plan under exeution. Will defer the new plan"); - plan_queue.push_back(goal->plan); - return rclcpp_action::GoalResponse::ACCEPT_AND_DEFER; - } - else - { - status_->info("Plan parsed and accepted"); - plan_queue.push_back(goal->plan); - return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; - } + status_->info("Plan parsed and accepted"); + + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; } /** @@ -163,8 +143,6 @@ class CapabilitiesFabric : public rclcpp::Node bond_client_->stop(); - fabric_state = Status::CANCELED; - return rclcpp_action::CancelResponse::ACCEPT; } @@ -177,17 +155,9 @@ class CapabilitiesFabric : public rclcpp::Node { goal_handle_ = goal_handle; - fabric_state = Status::RUNNING; - execution(); } - void setCompleteCallback(const std::shared_ptr request, std::shared_ptr response) - { - fabric_completed_ = true; - cv_.notify_all(); - } - /** * @brief Start the execution of the capabilities2 fabric */ @@ -195,9 +165,6 @@ class CapabilitiesFabric : public rclcpp::Node { process_feedback("A new execution started"); - lock_.lock(); - fabric_completed_ = false; - expected_providers_ = 0; completed_providers_ = 0; @@ -210,22 +177,7 @@ class CapabilitiesFabric : public rclcpp::Node expected_configurations_ = 0; completed_configurations_ = 0; - std::string plan_to_be_executed = plan_queue[0]; - - tinyxml2::XMLError xml_status = document.Parse(plan_to_be_executed.c_str()); - - // check if the file parsing failed - if (xml_status != tinyxml2::XMLError::XML_SUCCESS) - { - status_->error("Parsing the plan from queue failed"); - return; - } - else - { - status_->error("Parsing the plan from queue successful"); - plan_queue.pop_front(); - getInterfaces(); - } + getInterfaces(); } /** @@ -639,8 +591,8 @@ class CapabilitiesFabric : public rclcpp::Node } else { - request_configure->target_on_start.capability = "std_capabilities/FabricCompletionRunner"; - request_configure->target_on_start.provider = "std_capabilities/FabricCompletionRunner"; + request_configure->target_on_start.capability = ""; + request_configure->target_on_start.provider = ""; } if (xml_parser::convert_to_string(capabilities[completed_configurations_].target_on_stop.parameters, @@ -651,8 +603,8 @@ class CapabilitiesFabric : public rclcpp::Node } else { - request_configure->target_on_stop.capability = "std_capabilities/FabricCompletionRunner"; - request_configure->target_on_stop.provider = "std_capabilities/FabricCompletionRunner"; + request_configure->target_on_stop.capability = ""; + request_configure->target_on_stop.provider = ""; } if (xml_parser::convert_to_string(capabilities[completed_configurations_].target_on_success.parameters, @@ -663,8 +615,8 @@ class CapabilitiesFabric : public rclcpp::Node } else { - request_configure->target_on_success.capability = "std_capabilities/FabricCompletionRunner"; - request_configure->target_on_success.provider = "std_capabilities/FabricCompletionRunner"; + request_configure->target_on_success.capability = ""; + request_configure->target_on_success.provider = ""; } if (xml_parser::convert_to_string(capabilities[completed_configurations_].target_on_failure.parameters, @@ -675,8 +627,8 @@ class CapabilitiesFabric : public rclcpp::Node } else { - request_configure->target_on_failure.capability = "std_capabilities/FabricCompletionRunner"; - request_configure->target_on_failure.provider = "std_capabilities/FabricCompletionRunner"; + request_configure->target_on_failure.capability = ""; + request_configure->target_on_failure.provider = ""; } std::string source_capability = capabilities[completed_configurations_].source.runner; @@ -734,36 +686,8 @@ class CapabilitiesFabric : public rclcpp::Node auto response = future.get(); process_feedback("Successfully triggered capability " + connection_map[0].source.runner); - wait_for_completion(); - }); - } - - /** - * @brief Wait for the execution completion of the fabric - * - */ - void wait_for_completion() - { - process_feedback("Waiting for fabric execution completion"); - - // Conditional wait - while (!fabric_completed_) - { - cv_.wait(lock_); - } - - lock_.unlock(); - - if (plan_queue.size()>0) - { - process_feedback("Successfully completed capabilities2 fabric. Starting the next fabric"); - - execution(); - } - else - { process_result("Successfully completed capabilities2 fabric", true); - } + }); } void check_service(bool wait_for_logic, const std::string& service_name) @@ -807,13 +731,11 @@ class CapabilitiesFabric : public rclcpp::Node if (success) { status_->info(text, newline); - fabric_state = Status::SUCCEEDED; goal_handle_->succeed(result_msg); } else { status_->error(text, newline); - fabric_state = Status::ABORTED; goal_handle_->abort(result_msg); } } @@ -840,24 +762,15 @@ class CapabilitiesFabric : public rclcpp::Node /** Bond id */ std::string bond_id_; - /** Status of the fabric */ - Status fabric_state; - /** Manages bond between capabilities server and this client */ std::shared_ptr bond_client_; /** Handles status message sending and printing to logging */ std::shared_ptr status_; - /** Vector of plans */ - std::deque plan_queue; - /** XML Document */ tinyxml2::XMLDocument document; - /** XML Document */ - tinyxml2::XMLDocument documentEvaluation; - /** vector of connections */ std::map connection_map; @@ -912,12 +825,9 @@ class CapabilitiesFabric : public rclcpp::Node /** trigger an selected capability */ TriggerCapabilityClient::SharedPtr trig_capability_client_; - /** server to get the status of the capabilities2 fabric */ - rclcpp::Service::SharedPtr completion_server_; - /** capabilities2 server and fabric synchronization tools */ - std::mutex mutex_; - std::condition_variable cv_; - bool fabric_completed_; - std::unique_lock lock_; + // std::mutex mutex_; + // std::condition_variable cv_; + // bool fabric_completed_; + // std::unique_lock lock_; }; \ No newline at end of file diff --git a/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric_client.hpp b/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric_client.hpp index 0b5670a..d5462f8 100644 --- a/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric_client.hpp +++ b/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric_client.hpp @@ -2,6 +2,7 @@ #include #include #include +#include #include #include #include @@ -19,6 +20,7 @@ #include #include #include +#include /** * @brief Capabilities Executor File Parser @@ -30,7 +32,6 @@ class CapabilitiesFabricClient : public rclcpp::Node { public: - using Status = capabilities2::fabric_status; using Plan = capabilities2_msgs::action::Plan; using GoalHandlePlan = rclcpp_action::ClientGoalHandle; @@ -38,6 +39,7 @@ class CapabilitiesFabricClient : public rclcpp::Node using GetFabricStatus = capabilities2_msgs::srv::GetFabricStatus; using SetFabricPlan = capabilities2_msgs::srv::SetFabricPlan; using CancelFabricPlan = capabilities2_msgs::srv::CancelFabricPlan; + using CompleteFabric = capabilities2_msgs::srv::CompleteFabric; CapabilitiesFabricClient(const rclcpp::NodeOptions& options = rclcpp::NodeOptions()) : Node("Capabilities2_Fabric_Client", options) { @@ -66,6 +68,10 @@ class CapabilitiesFabricClient : public rclcpp::Node this->create_service("/capabilities_fabric/cancel_plan", std::bind(&CapabilitiesFabricClient::cancelPlanCallback, this, std::placeholders::_1, std::placeholders::_2)); + completion_server_ = + this->create_service("/capabilities_fabric/set_completion", std::bind(&CapabilitiesFabricClient::setCompleteCallback, this, + std::placeholders::_1, std::placeholders::_2)); + // Create the action client for capabilities_fabric after the node is fully constructed this->planner_client_ = rclcpp_action::create_client(shared_from_this(), "/capabilities_fabric"); @@ -90,22 +96,48 @@ class CapabilitiesFabricClient : public rclcpp::Node status_->info("Plan loaded from : " + plan_file_path); - send_goal(document); + std::string plan; + xml_parser::convert_to_string(document, plan); + + plan_queue.push_back(plan); + + goal_send_thread = std::thread(&CapabilitiesFabricClient::manage_goal, this); } private: - void send_goal(tinyxml2::XMLDocument& document_xml) + void manage_goal() + { + while (plan_queue.size() > 0) + { + status_->info("Fabric client thread starting"); + + std::unique_lock lock(mutex_); + completed_ = false; + + send_goal(); + + status_->info("Fabric plan sent. Waiting for acceptance."); + + // Conditional wait + cv_.wait(lock, [this] { return completed_; }); + status_->info("Fabric client thread closing"); + } + } + + void send_goal() { + // Create Plan Goal message auto goal_msg = Plan::Goal(); - xml_parser::convert_to_string(document_xml, goal_msg.plan); + // load the latest plan from the queue + goal_msg.plan = plan_queue[0]; + plan_queue.pop_front(); - status_->info("Following plan was loaded :\n\n " + goal_msg.plan); status_->info("Sending goal to the capabilities_fabric action server"); + // send goal options auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); - // send goal options // goal response callback send_goal_options.goal_response_callback = [this](const GoalHandlePlan::SharedPtr& goal_handle) { if (!goal_handle) @@ -115,7 +147,7 @@ class CapabilitiesFabricClient : public rclcpp::Node } else { - status_->info("Goal accepted by server, waiting for result"); + status_->info("Goal accepted by server, waiting for completion"); goal_handle_ = goal_handle; fabric_state = Status::RUNNING; } @@ -126,7 +158,7 @@ class CapabilitiesFabricClient : public rclcpp::Node switch (result.code) { case rclcpp_action::ResultCode::SUCCEEDED: - fabric_state = Status::SUCCEEDED; + fabric_state = Status::LAUNCHED; break; case rclcpp_action::ResultCode::ABORTED: status_->error("Goal was aborted"); @@ -163,6 +195,24 @@ class CapabilitiesFabricClient : public rclcpp::Node this->planner_client_->async_send_goal(goal_msg, send_goal_options); } + void cancelPlanCallback(const std::shared_ptr request, std::shared_ptr response) + { + if (fabric_state == Status::RUNNING) + { + this->planner_client_->async_cancel_goal(goal_handle_); + } + + response->success = true; + } + + void setCompleteCallback(const std::shared_ptr request, std::shared_ptr response) + { + fabric_state = Status::COMPLETED; + + completed_ = true; + cv_.notify_all(); + } + void getStatusCallback(const std::shared_ptr request, std::shared_ptr response) { if (fabric_state == Status::IDLE) @@ -185,9 +235,13 @@ class CapabilitiesFabricClient : public rclcpp::Node { response->status = GetFabricStatus::Response::FABRIC_FAILED; } - else if (fabric_state == Status::SUCCEEDED) + else if (fabric_state == Status::LAUNCHED) + { + response->status = GetFabricStatus::Response::FABRIC_LAUNCHED; + } + else if (fabric_state == Status::COMPLETED) { - response->status = GetFabricStatus::Response::FABRIC_SUCCEEDED; + response->status = GetFabricStatus::Response::FABRIC_COMPLETED; } else { @@ -200,7 +254,7 @@ class CapabilitiesFabricClient : public rclcpp::Node status_->info("Received the request with a plan"); // try to parse the std::string plan from capabilities_msgs/Plan to the to a XMLDocument file - tinyxml2::XMLError xml_status = document.Parse(request->plan.c_str()); + tinyxml2::XMLError xml_status = documentChecking.Parse(request->plan.c_str()); // check if the file parsing failed if (xml_status != tinyxml2::XMLError::XML_SUCCESS) @@ -209,18 +263,18 @@ class CapabilitiesFabricClient : public rclcpp::Node response->success = false; } - status_->info("Plan parsed and accepted"); + status_->info("Plan parsed and valid"); - response->success = true; + plan_queue.push_back(request->plan); - send_goal(document); - } - - void cancelPlanCallback(const std::shared_ptr request, std::shared_ptr response) - { if (fabric_state == Status::RUNNING) { - this->planner_client_->async_cancel_goal(goal_handle_); + status_->info("Prior plan under exeution. Will defer the new plan"); + } + else + { + status_->info("Plan parsed and accepted"); + goal_send_thread = std::thread(&CapabilitiesFabricClient::manage_goal, this); } response->success = true; @@ -233,9 +287,18 @@ class CapabilitiesFabricClient : public rclcpp::Node /** Status message */ std::string status; + /** Vector of plans */ + std::deque plan_queue; + /** XML Document */ tinyxml2::XMLDocument document; + /** XML Document */ + tinyxml2::XMLDocument documentChecking; + + /** Thread to manage sending goal */ + std::thread goal_send_thread; + /** action client */ rclcpp_action::Client::SharedPtr planner_client_; @@ -254,6 +317,24 @@ class CapabilitiesFabricClient : public rclcpp::Node /** server to cancel the current plan in the capabilities2 fabric */ rclcpp::Service::SharedPtr cancel_server_; + /** server to get the status of the capabilities2 fabric */ + rclcpp::Service::SharedPtr completion_server_; + /** Status of the fabric */ Status fabric_state; + + /** + * @brief mutex for threadpool synchronisation. + */ + std::mutex mutex_; + + /** + * @brief conditional variable for threadpool synchronisation. + */ + std::condition_variable cv_; + + /** + * @brief flag for threadpool synchronisation. + */ + bool completed_; }; diff --git a/capabilities2_fabric/include/capabilities2_fabric/utils/fabric_status.hpp b/capabilities2_fabric/include/capabilities2_fabric/utils/fabric_status.hpp index bd3fffb..e862452 100644 --- a/capabilities2_fabric/include/capabilities2_fabric/utils/fabric_status.hpp +++ b/capabilities2_fabric/include/capabilities2_fabric/utils/fabric_status.hpp @@ -9,6 +9,7 @@ enum fabric_status CANCELED, ABORTED, FAILED, - SUCCEEDED + LAUNCHED, + COMPLETED }; } \ No newline at end of file diff --git a/capabilities2_fabric/include/capabilities2_fabric/utils/xml_parser.hpp b/capabilities2_fabric/include/capabilities2_fabric/utils/xml_parser.hpp index 71f450d..aae33d7 100644 --- a/capabilities2_fabric/include/capabilities2_fabric/utils/xml_parser.hpp +++ b/capabilities2_fabric/include/capabilities2_fabric/utils/xml_parser.hpp @@ -20,6 +20,40 @@ namespace xml_parser return document.FirstChildElement("Plan")->FirstChildElement(); } + /** + * @brief add std_capabilities/FabricCompletionRunner into the plan + * as an internal mechanism + * + * @param document XML document to modify + * + * @return modified document + */ + void add_closing_event(tinyxml2::XMLDocument &document) + { + // Get the root element + tinyxml2::XMLElement* plan = document.FirstChildElement("Plan"); + + // Get the element + tinyxml2::XMLElement* innerControl = plan->FirstChildElement("Control"); + + // Create the outer element + tinyxml2::XMLElement* outerControl = document.NewElement("Control"); + outerControl->SetAttribute("name", "sequential"); + + // Remove the inner from and move it inside the new outer + plan->DeleteChild(innerControl); + outerControl->InsertEndChild(innerControl); + + // Create and append the new element + tinyxml2::XMLElement* newEvent = document.NewElement("Event"); + newEvent->SetAttribute("name", "std_capabilities/FabricCompletionRunner"); + newEvent->SetAttribute("provider", "std_capabilities/FabricCompletionRunner"); + outerControl->InsertEndChild(newEvent); + + // Append the outer to + plan->InsertEndChild(outerControl); + } + /** * @brief search a string in a vector of strings * diff --git a/capabilities2_msgs/srv/GetFabricStatus.srv b/capabilities2_msgs/srv/GetFabricStatus.srv index 6e12c28..675f947 100644 --- a/capabilities2_msgs/srv/GetFabricStatus.srv +++ b/capabilities2_msgs/srv/GetFabricStatus.srv @@ -7,5 +7,6 @@ uint8 FABRIC_RUNNING=1 uint8 FABRIC_CANCELED=2 uint8 FABRIC_ABORTED=3 uint8 FABRIC_FAILED=4 -uint8 FABRIC_SUCCEEDED=5 -uint8 UNKNOWN=6 \ No newline at end of file +uint8 FABRIC_LAUNCHED=5 +uint8 FABRIC_COMPLETED=6 +uint8 UNKNOWN=7 \ No newline at end of file From 3749b53102f9a49cdbbdc5ef438ffd6b09b34f00 Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Thu, 6 Feb 2025 01:01:02 +1100 Subject: [PATCH 084/133] fixed issues in the recursive functions used for extracting connections in the xml_parser::extract_cconnections --- .../capabilities_fabric.hpp | 8 + .../capabilities2_fabric/utils/xml_parser.hpp | 517 +++++++++--------- .../capabilities2_server/runner_cache.hpp | 2 +- 3 files changed, 264 insertions(+), 263 deletions(-) diff --git a/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric.hpp b/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric.hpp index 454a698..04c9120 100644 --- a/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric.hpp +++ b/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric.hpp @@ -165,6 +165,14 @@ class CapabilitiesFabric : public rclcpp::Node { process_feedback("A new execution started"); + xml_parser::add_closing_event(document); + + std::string modified_plan; + + xml_parser::convert_to_string(document, modified_plan); + + status_->info("Plan after adding closing event :\n\n " + modified_plan); + expected_providers_ = 0; completed_providers_ = 0; diff --git a/capabilities2_fabric/include/capabilities2_fabric/utils/xml_parser.hpp b/capabilities2_fabric/include/capabilities2_fabric/utils/xml_parser.hpp index aae33d7..fc4fba1 100644 --- a/capabilities2_fabric/include/capabilities2_fabric/utils/xml_parser.hpp +++ b/capabilities2_fabric/include/capabilities2_fabric/utils/xml_parser.hpp @@ -8,322 +8,315 @@ namespace xml_parser { - /** - * @brief extract elements related plan - * - * @param document XML document to extract plan from - * - * @return plan in the form of tinyxml2::XMLElement* - */ - tinyxml2::XMLElement *get_plan(tinyxml2::XMLDocument &document) - { - return document.FirstChildElement("Plan")->FirstChildElement(); - } - - /** - * @brief add std_capabilities/FabricCompletionRunner into the plan - * as an internal mechanism - * - * @param document XML document to modify - * - * @return modified document - */ - void add_closing_event(tinyxml2::XMLDocument &document) - { - // Get the root element - tinyxml2::XMLElement* plan = document.FirstChildElement("Plan"); - - // Get the element - tinyxml2::XMLElement* innerControl = plan->FirstChildElement("Control"); - - // Create the outer element - tinyxml2::XMLElement* outerControl = document.NewElement("Control"); - outerControl->SetAttribute("name", "sequential"); - - // Remove the inner from and move it inside the new outer - plan->DeleteChild(innerControl); - outerControl->InsertEndChild(innerControl); - - // Create and append the new element - tinyxml2::XMLElement* newEvent = document.NewElement("Event"); - newEvent->SetAttribute("name", "std_capabilities/FabricCompletionRunner"); - newEvent->SetAttribute("provider", "std_capabilities/FabricCompletionRunner"); - outerControl->InsertEndChild(newEvent); - - // Append the outer to - plan->InsertEndChild(outerControl); - } - - /** - * @brief search a string in a vector of strings - * - * @param list vector of strings to be searched - * @param value string to be searched in the vector - * - * @return `true` if value is found in list and `false` otherwise - */ - bool search(std::vector list, std::string value) - { - return (std::find(list.begin(), list.end(), value) != list.end()); - } - - /** - * @brief check if the element in question is the last in its level - * - * @param element element to be checked - * - * @return `true` if last and `false` otherwise - */ - bool isLastElement(tinyxml2::XMLElement *element) - { - return (element == element->Parent()->LastChildElement()); - } - - /** - * @brief check if the xml document has valid plan tags - * - * @param document XML document in question - * - * @return `true` if its valid and `false` otherwise - */ - bool check_plan_tag(tinyxml2::XMLDocument &document) +/** + * @brief extract elements related plan + * + * @param document XML document to extract plan from + * + * @return plan in the form of tinyxml2::XMLElement* + */ +tinyxml2::XMLElement* get_plan(tinyxml2::XMLDocument& document) +{ + return document.FirstChildElement("Plan")->FirstChildElement(); +} + +/** + * @brief search a string in a vector of strings + * + * @param list vector of strings to be searched + * @param value string to be searched in the vector + * + * @return `true` if value is found in list and `false` otherwise + */ +bool search(std::vector list, std::string value) +{ + return (std::find(list.begin(), list.end(), value) != list.end()); +} + +/** + * @brief check if the xml document has valid plan tags + * + * @param document XML document in question + * + * @return `true` if its valid and `false` otherwise + */ +bool check_plan_tag(tinyxml2::XMLDocument& document) +{ + std::string plan_tag(document.FirstChildElement()->Name()); + + if (plan_tag == "Plan") + return true; + else + return false; +} + +/** + * @brief convert XMLElement to std::string + * + * @param element XMLElement element to be converted + * @param paramters parameter to hold std::string + * + * @return `true` if element is not nullptr and conversion successful, `false` if element is nullptr + */ +bool convert_to_string(tinyxml2::XMLElement* element, std::string& parameters) +{ + if (element) { - std::string plan_tag(document.FirstChildElement()->Name()); - - if (plan_tag == "Plan") - return true; - else - return false; + tinyxml2::XMLPrinter printer; + element->Accept(&printer); + parameters = printer.CStr(); + return true; } - - /** - * @brief convert XMLElement to std::string - * - * @param element XMLElement element to be converted - * @param paramters parameter to hold std::string - * - * @return `true` if element is not nullptr and conversion successful, `false` if element is nullptr - */ - bool convert_to_string(tinyxml2::XMLElement *element, std::string ¶meters) + else { - if (element) - { - tinyxml2::XMLPrinter printer; - element->Accept(&printer); - parameters = printer.CStr(); - return true; - } - else - { - parameters = ""; - return false; - } + parameters = ""; + return false; } +} + +/** + * @brief convert XMLDocument to std::string + * + * @param document element to be converted + * + * @return std::string converted document + */ +void convert_to_string(tinyxml2::XMLDocument& document_xml, std::string& document_string) +{ + tinyxml2::XMLPrinter printer; + document_xml.Print(&printer); + document_string = printer.CStr(); +} - /** - * @brief convert XMLDocument to std::string - * - * @param document element to be converted - * - * @return std::string converted document - */ - void convert_to_string(tinyxml2::XMLDocument &document_xml, std::string &document_string) - { - tinyxml2::XMLPrinter printer; - document_xml.Print(&printer); - document_string = printer.CStr(); - } +void add_closing_event(tinyxml2::XMLDocument& document) +{ + // Get the root element + tinyxml2::XMLElement* plan = document.FirstChildElement("Plan"); + + // Get the existing element inside + tinyxml2::XMLElement* innerControl = plan->FirstChildElement("Control"); + + // Create the outer element + tinyxml2::XMLElement* outerControl = document.NewElement("Control"); + outerControl->SetAttribute("name", "sequential"); + + // Clone the existing element instead of deleting it + tinyxml2::XMLElement* clonedControl = innerControl->DeepClone(&document)->ToElement(); + + // Insert the cloned inner control inside the new outer control + outerControl->InsertEndChild(clonedControl); + + // Create and append the new element + tinyxml2::XMLElement* newEvent = document.NewElement("Event"); + newEvent->SetAttribute("name", "std_capabilities/FabricCompletionRunner"); + newEvent->SetAttribute("provider", "std_capabilities/FabricCompletionRunner"); + outerControl->InsertEndChild(newEvent); + + // Remove the original innerControl (after cloning) + plan->DeleteChild(innerControl); + + // Append the new outer control to + plan->InsertEndChild(outerControl); +} + +/** + * @brief check the plan for invalid/unsupported control and event tags + * uses recursive approach to go through the plan + * + * @param status StatusClient used for logging and status publishing + * @param element XML Element to be evaluated + * @param events list containing valid event tags + * @param providers list containing providers + * @param control list containing valid control tags + * @param rejected list containing invalid tags + * + * @return `true` if element valid and supported and `false` otherwise + */ +bool check_tags(const std::shared_ptr status, tinyxml2::XMLElement* element, std::vector& events, + std::vector& providers, std::vector& control, std::vector& rejected) +{ + const char* name; + const char* provider; - /** - * @brief check the plan for invalid/unsupported control and event tags - * uses recursive approach to go through the plan - * - * @param status StatusClient used for logging and status publishing - * @param element XML Element to be evaluated - * @param events list containing valid event tags - * @param providers list containing providers - * @param control list containing valid control tags - * @param rejected list containing invalid tags - * - * @return `true` if element valid and supported and `false` otherwise - */ - bool check_tags(const std::shared_ptr status, tinyxml2::XMLElement *element, std::vector &events, std::vector &providers, - std::vector &control, std::vector &rejected) - { - const char *name; - const char *provider; + std::string parameter_string; + convert_to_string(element, parameter_string); - std::string parameter_string; - convert_to_string(element, parameter_string); + element->QueryStringAttribute("name", &name); + element->QueryStringAttribute("provider", &provider); - element->QueryStringAttribute("name", &name); - element->QueryStringAttribute("provider", &provider); + std::string nametag; + std::string providertag; - std::string nametag; - std::string providertag; + std::string typetag(element->Name()); - std::string typetag(element->Name()); + if (name) + nametag = name; + else + nametag = ""; - if (name) - nametag = name; - else - nametag = ""; + if (provider) + providertag = provider; + else + providertag = ""; - if (provider) - providertag = provider; - else - providertag = ""; + bool hasChildren = !element->NoChildren(); + bool hasSiblings = (element->NextSiblingElement() != nullptr); + bool foundInControl = xml_parser::search(control, nametag); + bool foundInEvents = xml_parser::search(events, nametag); + bool foundInProviders = xml_parser::search(providers, providertag); + bool returnValue = true; - bool hasChildren = !element->NoChildren(); - bool hasSiblings = !xml_parser::isLastElement(element); - bool foundInControl = xml_parser::search(control, nametag); - bool foundInEvents = xml_parser::search(events, nametag); - bool foundInProviders = xml_parser::search(providers, providertag); - bool returnValue = true; - - if (typetag == "Control") + if (typetag == "Control") + { + if (!foundInControl) { - if (!foundInControl) - { - std::string msg = "Control tag '" + nametag + "' not available in the valid list"; - status->error(msg); - rejected.push_back(parameter_string); - return false; - } + std::string msg = "Control tag '" + nametag + "' not available in the valid list"; + status->error(msg); + rejected.push_back(parameter_string); + return false; + } - if (hasChildren) - returnValue &= xml_parser::check_tags(status, element->FirstChildElement(), events, providers, control, rejected); + if (hasChildren) + returnValue &= xml_parser::check_tags(status, element->FirstChildElement(), events, providers, control, rejected); - if (hasSiblings) - returnValue &= xml_parser::check_tags(status, element->NextSiblingElement(), events, providers, control, rejected); - } - else if (typetag == "Event") - { - if (!foundInEvents || !foundInProviders) - { - std::string msg = "Event tag name '" + nametag + "' or provider '" + providertag + "' not available in the valid list"; - status->error(msg); - rejected.push_back(parameter_string); - return false; - } - - if (hasSiblings) - returnValue &= xml_parser::check_tags(status, element->NextSiblingElement(), events, providers, control, rejected); - } - else + if (hasSiblings) + returnValue &= xml_parser::check_tags(status, element->NextSiblingElement(), events, providers, control, rejected); + } + else if (typetag == "Event") + { + if (!foundInEvents || !foundInProviders) { - std::string msg = "XML element is not valid :" + parameter_string; + std::string msg = "Event tag name '" + nametag + "' or provider '" + providertag + "' not available in the valid list"; status->error(msg); rejected.push_back(parameter_string); return false; } - return returnValue; + if (hasSiblings) + returnValue &= xml_parser::check_tags(status, element->NextSiblingElement(), events, providers, control, rejected); } - - /** - * @brief Returns control xml tags supported in extract_connections method - * - */ - std::vector get_control_list() + else { - std::vector tag_list; - - tag_list.push_back("sequential"); - tag_list.push_back("parallel"); - tag_list.push_back("recovery"); - - return tag_list; + std::string msg = "XML element is not valid :" + parameter_string; + status->error(msg); + rejected.push_back(parameter_string); + return false; } - /** - * @brief parse through the plan and extract the connections - * - * @param element XML Element to be evaluated - * @param connections std::map containing extracted connections - * @param connection_id numerical id of the connection - * @param connection_type the type of connection - */ - void extract_connections(tinyxml2::XMLElement *element, std::map &connections, int connection_id = 0, - capabilities2::connection_type_t connection_type = capabilities2::connection_type_t::ON_SUCCESS) - { - int predecessor_id; + return returnValue; +} - const char *name; - const char *provider; +/** + * @brief Returns control xml tags supported in extract_connections method + * + */ +std::vector get_control_list() +{ + std::vector tag_list; + + tag_list.push_back("sequential"); + tag_list.push_back("parallel"); + tag_list.push_back("recovery"); + + return tag_list; +} + +/** + * @brief parse through the plan and extract the connections + * + * @param element XML Element to be evaluated + * @param connections std::map containing extracted connections + * @param connection_id numerical id of the connection + * @param connection_type the type of connection + */ +int extract_connections(tinyxml2::XMLElement* element, std::map& connections, int connection_id = 0, + capabilities2::connection_type_t connection_type = capabilities2::connection_type_t::ON_SUCCESS) +{ + int predecessor_id; - element->QueryStringAttribute("name", &name); - element->QueryStringAttribute("provider", &provider); + const char* name = element->Attribute("name"); + const char* provider = element->Attribute("provider"); - std::string typetag(element->Name()); + std::string typetag(element->Name()); - std::string nametag; - std::string providertag; + std::string nametag; + std::string providertag; - if (name) - nametag = name; - else - nametag = ""; + if (name) + nametag = name; + else + nametag = ""; - if (provider) - providertag = provider; - else - providertag = ""; + if (provider) + providertag = provider; + else + providertag = ""; - bool hasChildren = !element->NoChildren(); - bool hasSiblings = !xml_parser::isLastElement(element); + bool hasChildren = (element->FirstChildElement() != nullptr); + bool hasSiblings = (element->NextSiblingElement() != nullptr); - if ((typetag == "Control") and (nametag == "sequential")) + if (typetag == "Control") + { + if (nametag == "sequential") { if (hasChildren) - xml_parser::extract_connections(element->FirstChildElement(), connections, connection_id, capabilities2::connection_type_t::ON_SUCCESS); - - if (hasSiblings) - xml_parser::extract_connections(element->NextSiblingElement(), connections, connection_id, connection_type); + predecessor_id = + xml_parser::extract_connections(element->FirstChildElement(), connections, connection_id, capabilities2::connection_type_t::ON_SUCCESS); } - else if ((typetag == "Control") and (nametag == "parallel")) + else if (nametag == "parallel") { if (hasChildren) - xml_parser::extract_connections(element->FirstChildElement(), connections, connection_id, capabilities2::connection_type_t::ON_START); - - if (hasSiblings) - xml_parser::extract_connections(element->NextSiblingElement(), connections, connection_id, connection_type); + predecessor_id = + xml_parser::extract_connections(element->FirstChildElement(), connections, connection_id, capabilities2::connection_type_t::ON_START); } - else if ((typetag == "Control") and (nametag == "recovery")) + else if (nametag == "recovery") { if (hasChildren) - xml_parser::extract_connections(element->FirstChildElement(), connections, connection_id, capabilities2::connection_type_t::ON_FAILURE); - - if (hasSiblings) - xml_parser::extract_connections(element->NextSiblingElement(), connections, connection_id, connection_type); + predecessor_id = + xml_parser::extract_connections(element->FirstChildElement(), connections, connection_id, capabilities2::connection_type_t::ON_FAILURE); } - else if (typetag == "Event") + + if (hasSiblings) { - capabilities2::node_t node; + predecessor_id = xml_parser::extract_connections(element->NextSiblingElement(), connections, predecessor_id+1, connection_type); + } - node.source.runner = nametag; - node.source.provider = providertag; - node.source.parameters = element; + return predecessor_id; + } + else if (typetag == "Event") + { + capabilities2::node_t node; - predecessor_id = connection_id - 1; + node.source.runner = nametag; + node.source.provider = providertag; + node.source.parameters = element; - while (connections.count(connection_id) > 0) - connection_id += 1; + predecessor_id = connection_id - 1; - connections[connection_id] = node; + while (connections.count(connection_id) > 0) + connection_id += 1; - if ((connection_type == capabilities2::connection_type_t::ON_SUCCESS) and (connection_id != 0)) + connections[connection_id] = node; + + if (connection_id != 0) + { + if (connection_type == capabilities2::connection_type_t::ON_SUCCESS) connections[predecessor_id].target_on_success = connections[connection_id].source; - else if ((connection_type == capabilities2::connection_type_t::ON_START) and (connection_id != 0)) + else if (connection_type == capabilities2::connection_type_t::ON_START) connections[predecessor_id].target_on_start = connections[connection_id].source; - else if ((connection_type == capabilities2::connection_type_t::ON_FAILURE) and (connection_id != 0)) + else if (connection_type == capabilities2::connection_type_t::ON_FAILURE) connections[predecessor_id].target_on_failure = connections[connection_id].source; - - if (hasSiblings) - xml_parser::extract_connections(element->NextSiblingElement(), connections, connection_id + 1, connection_type); } + + if (hasSiblings) + predecessor_id = extract_connections(element->NextSiblingElement(), connections, connection_id + 1, connection_type); + else + predecessor_id += 1; // connection_id + + return predecessor_id; } +} -} // namespace xml_parser +} // namespace xml_parser diff --git a/capabilities2_server/include/capabilities2_server/runner_cache.hpp b/capabilities2_server/include/capabilities2_server/runner_cache.hpp index af1a2e3..784334f 100644 --- a/capabilities2_server/include/capabilities2_server/runner_cache.hpp +++ b/capabilities2_server/include/capabilities2_server/runner_cache.hpp @@ -152,7 +152,7 @@ class RunnerCache event_options, std::bind(&capabilities2_server::RunnerCache::trigger_runner, this, std::placeholders::_1, std::placeholders::_2)); - print_("Configured triggers for capability" + capability + ": \n\tStarted: " + on_started_capability + + print_("Configured triggers for capability " + capability + ": \n\tStarted: " + on_started_capability + " \n\tFailure: " + on_failure_capability + " \n\tSuccess: " + on_success_capability + "\n\tStopped: " + on_stopped_capability + "\n", true, false); } From 43bf4fa246de83e82c3f096df10cce0bf9978721 Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Sat, 8 Feb 2025 17:31:26 +1100 Subject: [PATCH 085/133] added more runner for capabilities --- capabilities2_msgs/msg/CapabilityEvent.msg | 6 + .../capabilities2_runner/runner_base.hpp | 18 +++ .../capability_get_runner.hpp | 91 +++++++++++++++ .../fabric_set_plan_runner.hpp | 62 ++++++++++ .../src/capabilities2_runner.cpp | 6 +- .../occupancygrid_runner.hpp | 37 +++++- .../robotpose_runner.hpp | 16 ++- .../prompt_capability_runner.hpp | 63 ++++++++++ .../prompt_occupancy_runner.hpp | 5 +- .../prompt_plan_response_runner.hpp | 109 ------------------ ...uest_runner.hpp => prompt_plan_runner.hpp} | 4 +- .../prompt_pose_runner.hpp | 6 +- .../src/prompt_runners.cpp | 4 +- 13 files changed, 306 insertions(+), 121 deletions(-) create mode 100644 capabilities2_runner_capabilities/include/capabilities2_runner_capabilities/capability_get_runner.hpp create mode 100644 capabilities2_runner_capabilities/include/capabilities2_runner_capabilities/fabric_set_plan_runner.hpp create mode 100644 capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_capability_runner.hpp delete mode 100644 capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_response_runner.hpp rename capabilities2_runner_prompt/include/capabilities2_runner_prompt/{prompt_plan_request_runner.hpp => prompt_plan_runner.hpp} (96%) diff --git a/capabilities2_msgs/msg/CapabilityEvent.msg b/capabilities2_msgs/msg/CapabilityEvent.msg index 581a67d..baf45b4 100644 --- a/capabilities2_msgs/msg/CapabilityEvent.msg +++ b/capabilities2_msgs/msg/CapabilityEvent.msg @@ -12,6 +12,9 @@ int8 thread_id # status message string text +# status message +string element + # Event types uint8 IDLE=0 uint8 STARTED=1 @@ -29,5 +32,8 @@ bool server_ready # Whether a failure occured bool error +# Whether output data +bool is_element + # PID of the related process int32 pid diff --git a/capabilities2_runner/include/capabilities2_runner/runner_base.hpp b/capabilities2_runner/include/capabilities2_runner/runner_base.hpp index 11e4ec2..e4f81cb 100644 --- a/capabilities2_runner/include/capabilities2_runner/runner_base.hpp +++ b/capabilities2_runner/include/capabilities2_runner/runner_base.hpp @@ -529,6 +529,24 @@ class RunnerBase print_(message); } + void output_(const std::string text, const std::string element, int thread_id = -1) + { + auto message = Event(); + + message.header.stamp = rclcpp::Clock().now(); + message.source.capability = run_config_.interface; + message.source.provider = run_config_.provider; + message.thread_id = thread_id; + message.text = text; + message.element = element; + message.is_element = true; + message.error = false; + message.server_ready = true; + message.event = Event::IDLE; + + print_(message); + } + /** * @brief shared pointer to the capabilities node. Allows to use ros node related functionalities */ diff --git a/capabilities2_runner_capabilities/include/capabilities2_runner_capabilities/capability_get_runner.hpp b/capabilities2_runner_capabilities/include/capabilities2_runner_capabilities/capability_get_runner.hpp new file mode 100644 index 0000000..49c340f --- /dev/null +++ b/capabilities2_runner_capabilities/include/capabilities2_runner_capabilities/capability_get_runner.hpp @@ -0,0 +1,91 @@ +#pragma once + +#include + +#include +#include +#include + +#include +#include + +namespace capabilities2_runner +{ + +/** + * @brief Executor runner class + * + * Class to run capabilities2 executor action based capability + * + */ +class CapabilityGetRunner : public ServiceRunner +{ +public: + CapabilityGetRunner() : ServiceRunner() + { + } + + /** + * @brief Starter function for starting the action runner + * + * @param node shared pointer to the capabilities node. Allows to use ros node related functionalities + * @param run_config runner configuration loaded from the yaml file + */ + virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config, + std::function print) override + { + init_service(node, run_config, "/capabilities/get_capability_specs", print); + } + +protected: + /** + * @brief This generate goal function overrides the generate_goal() function from ActionRunner() + * @param parameters XMLElement that contains parameters in the format + '' + * @return ActionT::Goal the generated goal + */ + virtual capabilities2_msgs::srv::GetCapabilitySpecs::Request generate_request(tinyxml2::XMLElement* parameters, + int id) override + { + capabilities2_msgs::srv::GetCapabilitySpecs::Request request; + return request; + } + + virtual std::string update_on_success(std::string& parameters) + { + tinyxml2::XMLElement* element = convert_to_xml(parameters); + + // Create the OccupancyGrid element as a child of the existing parameters element + tinyxml2::XMLElement* capabilitySpecsElement = element->GetDocument()->NewElement("CapabilitySpecs"); + element->InsertEndChild(capabilitySpecsElement); + + for (const auto& spec : response_->capability_specs) + { + // Create a element + tinyxml2::XMLElement* specElement = element->GetDocument()->NewElement("CapabilitySpec"); + + // Set attributes + specElement->SetAttribute("package", spec.package.c_str()); + specElement->SetAttribute("type", spec.type.c_str()); + + if (!spec.default_provider.empty()) + { + specElement->SetAttribute("default_provider", spec.default_provider.c_str()); + } + + // Add content as a child element inside + tinyxml2::XMLElement* contentElement = element->GetDocument()->NewElement("content"); + contentElement->SetText(spec.content.c_str()); + specElement->InsertEndChild(contentElement); + + // Append to + capabilitySpecsElement->InsertEndChild(specElement); + } + + // Convert updated XML back to string and return + std::string result = convert_to_string(element); + return result; + } +}; + +} // namespace capabilities2_runner diff --git a/capabilities2_runner_capabilities/include/capabilities2_runner_capabilities/fabric_set_plan_runner.hpp b/capabilities2_runner_capabilities/include/capabilities2_runner_capabilities/fabric_set_plan_runner.hpp new file mode 100644 index 0000000..3d83b04 --- /dev/null +++ b/capabilities2_runner_capabilities/include/capabilities2_runner_capabilities/fabric_set_plan_runner.hpp @@ -0,0 +1,62 @@ +#pragma once + +#include + +#include +#include +#include + +#include +#include + +namespace capabilities2_runner +{ + +/** + * @brief Executor runner class + * + * Class to run capabilities2 executor action based capability + * + */ +class FabricSetPlanRunner : public ServiceRunner +{ +public: + FabricSetPlanRunner() : ServiceRunner() + { + } + + /** + * @brief Starter function for starting the action runner + * + * @param node shared pointer to the capabilities node. Allows to use ros node related functionalities + * @param run_config runner configuration loaded from the yaml file + */ + virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config, + std::function print) override + { + init_service(node, run_config, "/capabilities_fabric/set_plan", print); + } + +protected: + /** + * @brief This generate goal function overrides the generate_goal() function from ActionRunner() + * @param parameters XMLElement that contains parameters in the format + * @return ActionT::Goal the generated goal + */ + virtual capabilities2_msgs::srv::SetFabricPlan::Request generate_request(tinyxml2::XMLElement* parameters, int id) override + { + tinyxml2::XMLElement* planElement = parameters->FirstChildElement("ReceievdPlan"); + + capabilities2_msgs::srv::SetFabricPlan::Request request; + + // Check if the element was found and has text content + if (planElement && planElement->GetText()) + { + request.plan = std::string(planElement->GetText()); + } + + return request; + } +}; + +} // namespace capabilities2_runner diff --git a/capabilities2_runner_capabilities/src/capabilities2_runner.cpp b/capabilities2_runner_capabilities/src/capabilities2_runner.cpp index 26e1b95..39c18de 100644 --- a/capabilities2_runner_capabilities/src/capabilities2_runner.cpp +++ b/capabilities2_runner_capabilities/src/capabilities2_runner.cpp @@ -1,6 +1,10 @@ #include #include #include +#include +#include // register runner plugins -PLUGINLIB_EXPORT_CLASS(capabilities2_runner::FabricCompletionRunner, capabilities2_runner::RunnerBase) \ No newline at end of file +PLUGINLIB_EXPORT_CLASS(capabilities2_runner::FabricCompletionRunner, capabilities2_runner::RunnerBase) +PLUGINLIB_EXPORT_CLASS(capabilities2_runner::FabricSetPlanRunner, capabilities2_runner::RunnerBase) +PLUGINLIB_EXPORT_CLASS(capabilities2_runner::CapabilityGetRunner, capabilities2_runner::RunnerBase) diff --git a/capabilities2_runner_nav2/include/capabilities2_runner_nav2/occupancygrid_runner.hpp b/capabilities2_runner_nav2/include/capabilities2_runner_nav2/occupancygrid_runner.hpp index d95b9f3..9e658e1 100644 --- a/capabilities2_runner_nav2/include/capabilities2_runner_nav2/occupancygrid_runner.hpp +++ b/capabilities2_runner_nav2/include/capabilities2_runner_nav2/occupancygrid_runner.hpp @@ -37,12 +37,43 @@ class OccupancyGridRunner : public TopicRunner protected: /** - * @brief Update on_success event parameters with new data if avaible. + * @brief Update on_success event parameters with new data from an OccupancyGrid message if avaible. * * This function is used to inject new data into the XMLElement containing * parameters related to the on_success trigger event * - * A pattern needs to be implemented in the derived class + * + *
+ * + * 1700000000 + * 123456789 + * + * map + *
+ * + * 0.05 + * 100 + * 100 + * + * + * 1.0 + * 2.0 + * 0.0 + * + * + * 0.0 + * 0.0 + * 0.0 + * 1.0 + * + * + * + * 1700000000 + * 987654321 + * + * + * 0 1 2 3 4 5 6 7 8 9 + * * * @param parameters pointer to the XMLElement containing parameters * @return pointer to the XMLElement containing updated parameters @@ -154,6 +185,8 @@ class OccupancyGridRunner : public TopicRunner // Return the updated parameters element with OccupancyGrid added std::string result = convert_to_string(element); + output_("on_success trigger parameter", result); + return result; }; }; diff --git a/capabilities2_runner_nav2/include/capabilities2_runner_nav2/robotpose_runner.hpp b/capabilities2_runner_nav2/include/capabilities2_runner_nav2/robotpose_runner.hpp index c68ccfd..ed963a3 100644 --- a/capabilities2_runner_nav2/include/capabilities2_runner_nav2/robotpose_runner.hpp +++ b/capabilities2_runner_nav2/include/capabilities2_runner_nav2/robotpose_runner.hpp @@ -42,7 +42,19 @@ class RobotPoseRunner : public TopicRunner + * + * 1.23 + * 4.56 + * 7.89 + * + * + * 0.12 + * 0.34 + * 0.56 + * 0.78 + * + * * * @param parameters pointer to the XMLElement containing parameters * @return pointer to the XMLElement containing updated parameters @@ -96,6 +108,8 @@ class RobotPoseRunner : public TopicRunner + +#include +#include +#include + +#include +#include + +namespace capabilities2_runner +{ + +/** + * @brief Executor runner class + * + * Class to run capabilities2 executor action based capability + * + */ +class FabricSetPlanRunner : public ServiceRunner +{ +public: + FabricSetPlanRunner() : ServiceRunner() + { + } + + /** + * @brief Starter function for starting the action runner + * + * @param node shared pointer to the capabilities node. Allows to use ros node related functionalities + * @param run_config runner configuration loaded from the yaml file + */ + virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config, + std::function print) override + { + init_service(node, run_config, "/capabilities_fabric/set_plan", print); + } + +protected: + /** + * @brief This generate goal function overrides the generate_goal() function from ActionRunner() + * @param parameters XMLElement that contains parameters in the format + '' + * @return ActionT::Goal the generated goal + */ + virtual capabilities2_msgs::srv::SetFabricPlan::Request generate_request(tinyxml2::XMLElement* parameters, int id) override + { + tinyxml2::XMLElement* planElement = parameters->FirstChildElement("ReceievdPlan"); + + capabilities2_msgs::srv::SetFabricPlan::Request request; + + // Check if the element was found and has text content + if (planElement && planElement->GetText()) + { + request.plan = std::string(planElement->GetText()); + } + + return request; + } +}; + +} // namespace capabilities2_runner diff --git a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_occupancy_runner.hpp b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_occupancy_runner.hpp index af21915..8a97b1f 100644 --- a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_occupancy_runner.hpp +++ b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_occupancy_runner.hpp @@ -57,8 +57,7 @@ class PromptOccupancyRunner : public ServiceRunner prompt_msgs::srv::Prompt::Request request; request.prompt.prompt = "The OccupancyGrid of the robot shows the surrounding environment of the robot. The data " - "is given as a ros2 nav_msgs occupancy_grid of which the content are " + - data; + "is given as a ros2 nav_msgs::msg::OccupancyGrid of which the content are " + data; prompt_msgs::msg::ModelOption modelOption1; modelOption1.key = "model"; @@ -73,6 +72,8 @@ class PromptOccupancyRunner : public ServiceRunner request.prompt.options.push_back(modelOption2); + info_("prompting with : " + request.prompt.prompt, id); + return request; } }; diff --git a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_response_runner.hpp b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_response_runner.hpp deleted file mode 100644 index 784c52d..0000000 --- a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_response_runner.hpp +++ /dev/null @@ -1,109 +0,0 @@ -#pragma once - -#include - -#include -#include -#include - -#include -#include - -namespace capabilities2_runner -{ - -/** - * @brief Executor runner class - * - * Class to run capabilities2 executor action based capability - * - */ -class PromptPlanResponseRunner : public ActionRunner -{ -public: - PromptPlanResponseRunner() : 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 - */ - virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config, - std::function print) override - { - init_action(node, run_config, "capabilities_fabric", print); - } - -protected: - /** - * @brief This generate goal function overrides the generate_goal() function from ActionRunner() - * @param parameters XMLElement that contains parameters in the format - '' - * @return ActionT::Goal the generated goal - */ - virtual capabilities2_msgs::action::Plan::Goal generate_goal(tinyxml2::XMLElement* parameters, int id) override - { - tinyxml2::XMLElement* planElement = parameters->FirstChildElement("ReceievdPlan"); - - auto goal_msg = capabilities2_msgs::action::Plan::Goal(); - - // Check if the element was found and has text content - if (planElement && planElement->GetText()) - { - goal_msg.plan = std::string(planElement->GetText()); - } - - return goal_msg; - } - - /** - * @brief This generate feedback function overrides the generate_feedback() function from ActionRunner() - * - * @param msg feedback message from the action server - * @return std::string of feedback information - */ - virtual std::string - generate_feedback(const typename capabilities2_msgs::action::Plan::Feedback::ConstSharedPtr msg) override - { - std::string feedback = msg->progress; - return feedback; - } - - /** - * @brief Update on_failure event parameters with new data if avaible. - * - * This function is used to inject new data into the XMLElement containing - * parameters related to the on_failure trigger event - * - * A pattern needs to be implemented in the derived class - * - * @param parameters pointer to the XMLElement containing parameters - * @return pointer to the XMLElement containing updated parameters - */ - virtual std::string update_on_failure(std::string& parameters) - { - tinyxml2::XMLElement* element = convert_to_xml(parameters); - - element->SetAttribute("replan", true); - - // Create the failed elements element as a child of the existing parameters element - tinyxml2::XMLElement* failedElements = element->GetDocument()->NewElement("FailedElements"); - element->InsertEndChild(failedElements); - - std::string failedElementsString = ""; - - for (const auto& element : result_->failed_elements) - failedElementsString += element; - - failedElements->SetText(failedElementsString.c_str()); - - std::string result = convert_to_string(element); - - return result; - }; -}; - -} // namespace capabilities2_runner diff --git a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_request_runner.hpp b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_runner.hpp similarity index 96% rename from capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_request_runner.hpp rename to capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_runner.hpp index 62935ac..ce88b40 100644 --- a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_request_runner.hpp +++ b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_runner.hpp @@ -15,10 +15,10 @@ namespace capabilities2_runner * call on the prompt_tools/prompt service, providing it as a capability that prompts * capabilitie plans values */ -class PromptPlanRequestRunner : public ServiceRunner +class PromptPlanRunner : public ServiceRunner { public: - PromptPlanRequestRunner() : ServiceRunner() + PromptPlanRunner() : ServiceRunner() { } diff --git a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_pose_runner.hpp b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_pose_runner.hpp index b7cb004..d5dff0a 100644 --- a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_pose_runner.hpp +++ b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_pose_runner.hpp @@ -56,8 +56,8 @@ class PromptPoseRunner : public ServiceRunner prompt_msgs::srv::Prompt::Request request; - request.prompt.prompt = - "The position of the robot is given as a standard ros2 geometry message of which the content are " + data; + request.prompt.prompt = "The position of the robot is given as a standard ros2 geometry_msgs::msg::Pose of which " + "the content are " + data; prompt_msgs::msg::ModelOption modelOption1; modelOption1.key = "model"; @@ -72,6 +72,8 @@ class PromptPoseRunner : public ServiceRunner request.prompt.options.push_back(modelOption2); + info_("prompting with : " + request.prompt.prompt, id); + return request; } }; diff --git a/capabilities2_runner_prompt/src/prompt_runners.cpp b/capabilities2_runner_prompt/src/prompt_runners.cpp index 361733f..080dda2 100644 --- a/capabilities2_runner_prompt/src/prompt_runners.cpp +++ b/capabilities2_runner_prompt/src/prompt_runners.cpp @@ -3,12 +3,12 @@ #include #include #include -#include +#include #include // register runner plugins PLUGINLIB_EXPORT_CLASS(capabilities2_runner::PromptTextRunner, capabilities2_runner::RunnerBase) PLUGINLIB_EXPORT_CLASS(capabilities2_runner::PromptPoseRunner, capabilities2_runner::RunnerBase) PLUGINLIB_EXPORT_CLASS(capabilities2_runner::PromptOccupancyRunner, capabilities2_runner::RunnerBase) -PLUGINLIB_EXPORT_CLASS(capabilities2_runner::PromptPlanRequestRunner, capabilities2_runner::RunnerBase) +PLUGINLIB_EXPORT_CLASS(capabilities2_runner::PromptPlanRunner, capabilities2_runner::RunnerBase) PLUGINLIB_EXPORT_CLASS(capabilities2_runner::PromptPlanResponseRunner, capabilities2_runner::RunnerBase) \ No newline at end of file From 76e3f8f5e9777aca7a2e48fc644381f503de92c3 Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Mon, 10 Feb 2025 14:53:41 +1100 Subject: [PATCH 086/133] updated various capabilities --- capabilities2_fabric/plans/default.xml | 6 +- capabilities2_fabric/plans/prompt_1.xml | 7 +++ capabilities2_fabric/plans/prompt_2.xml | 7 +++ capabilities2_fabric/plans/prompt_3.xml | 7 +++ capabilities2_fabric/plans/prompt_4.xml | 9 +++ .../capability_get_runner.hpp | 27 ++++++++ .../prompt_capability_runner.hpp | 63 ++++++++++++------- .../prompt_occupancy_runner.hpp | 14 ++--- .../prompt_plan_runner.hpp | 19 ++++-- .../prompt_pose_runner.hpp | 22 +++---- 10 files changed, 129 insertions(+), 52 deletions(-) create mode 100644 capabilities2_fabric/plans/prompt_1.xml create mode 100644 capabilities2_fabric/plans/prompt_2.xml create mode 100644 capabilities2_fabric/plans/prompt_3.xml create mode 100644 capabilities2_fabric/plans/prompt_4.xml diff --git a/capabilities2_fabric/plans/default.xml b/capabilities2_fabric/plans/default.xml index 40fdfd9..038690d 100644 --- a/capabilities2_fabric/plans/default.xml +++ b/capabilities2_fabric/plans/default.xml @@ -1,11 +1,13 @@ + + - - + + \ No newline at end of file diff --git a/capabilities2_fabric/plans/prompt_1.xml b/capabilities2_fabric/plans/prompt_1.xml new file mode 100644 index 0000000..08e399b --- /dev/null +++ b/capabilities2_fabric/plans/prompt_1.xml @@ -0,0 +1,7 @@ + + + + + + + \ No newline at end of file diff --git a/capabilities2_fabric/plans/prompt_2.xml b/capabilities2_fabric/plans/prompt_2.xml new file mode 100644 index 0000000..79a50da --- /dev/null +++ b/capabilities2_fabric/plans/prompt_2.xml @@ -0,0 +1,7 @@ + + + + + + + \ No newline at end of file diff --git a/capabilities2_fabric/plans/prompt_3.xml b/capabilities2_fabric/plans/prompt_3.xml new file mode 100644 index 0000000..5a81e17 --- /dev/null +++ b/capabilities2_fabric/plans/prompt_3.xml @@ -0,0 +1,7 @@ + + + + + + + \ No newline at end of file diff --git a/capabilities2_fabric/plans/prompt_4.xml b/capabilities2_fabric/plans/prompt_4.xml new file mode 100644 index 0000000..55d3155 --- /dev/null +++ b/capabilities2_fabric/plans/prompt_4.xml @@ -0,0 +1,9 @@ + + + + + + + + + \ No newline at end of file diff --git a/capabilities2_runner_capabilities/include/capabilities2_runner_capabilities/capability_get_runner.hpp b/capabilities2_runner_capabilities/include/capabilities2_runner_capabilities/capability_get_runner.hpp index 49c340f..89b03f1 100644 --- a/capabilities2_runner_capabilities/include/capabilities2_runner_capabilities/capability_get_runner.hpp +++ b/capabilities2_runner_capabilities/include/capabilities2_runner_capabilities/capability_get_runner.hpp @@ -51,6 +51,33 @@ class CapabilityGetRunner : public ServiceRunner + * name: Navigation dependencies: + * - MapServer + * - PathPlanner + * + * + * + * + * name: ObjectDetection + * dependencies: + * - Camera + * - ImageProcessor + * + * + * + * + * @param parameters + * @return std::string + */ + virtual std::string update_on_success(std::string& parameters) { tinyxml2::XMLElement* element = convert_to_xml(parameters); diff --git a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_capability_runner.hpp b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_capability_runner.hpp index 4ce82e8..ab8f43a 100644 --- a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_capability_runner.hpp +++ b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_capability_runner.hpp @@ -1,27 +1,25 @@ #pragma once - -#include - #include #include #include - #include -#include +#include +#include namespace capabilities2_runner { /** - * @brief Executor runner class - * - * Class to run capabilities2 executor action based capability + * @brief prompt capabilities runner * + * This class is a wrapper around the capabilities2 service runner and is used to pass + * data to prompt_tools/prompt service, providing it as a capability that prompts + * robot capabilities. */ -class FabricSetPlanRunner : public ServiceRunner +class PromptCapabilityRunner : public ServiceRunner { public: - FabricSetPlanRunner() : ServiceRunner() + PromptCapabilityRunner() : ServiceRunner() { } @@ -34,27 +32,46 @@ class FabricSetPlanRunner : public ServiceRunner print) override { - init_service(node, run_config, "/capabilities_fabric/set_plan", print); + init_service(node, run_config, "prompt", print); } protected: /** - * @brief This generate goal function overrides the generate_goal() function from ActionRunner() - * @param parameters XMLElement that contains parameters in the format - '' - * @return ActionT::Goal the generated goal + * @brief Generate a request from parameters given. + * + * This function is used in conjunction with the trigger function to inject type erased parameters + * into the typed action + * + * @param parameters + * @return prompt_msgs::srv::Prompt::Request the generated request */ - virtual capabilities2_msgs::srv::SetFabricPlan::Request generate_request(tinyxml2::XMLElement* parameters, int id) override + virtual prompt_msgs::srv::Prompt::Request generate_request(tinyxml2::XMLElement* parameters, int id) override { - tinyxml2::XMLElement* planElement = parameters->FirstChildElement("ReceievdPlan"); + tinyxml2::XMLElement* capabilitySpecsElement = parameters->FirstChildElement("CapabilitySpecs"); + + tinyxml2::XMLPrinter printer; + capabilitySpecsElement->Accept(&printer); + + std::string data(printer.CStr()); + + prompt_msgs::srv::Prompt::Request request; + + request.prompt.prompt = "The capabilities of the robot are given as follows" + data; + + prompt_msgs::msg::ModelOption modelOption1; + modelOption1.key = "model"; + modelOption1.value = "llama3.1:8b"; + + request.prompt.options.push_back(modelOption1); + + prompt_msgs::msg::ModelOption modelOption2; + modelOption2.key = "stream"; + modelOption2.value = "false"; + modelOption2.type = prompt_msgs::msg::ModelOption::BOOL_TYPE; - capabilities2_msgs::srv::SetFabricPlan::Request request; + request.prompt.options.push_back(modelOption2); - // Check if the element was found and has text content - if (planElement && planElement->GetText()) - { - request.plan = std::string(planElement->GetText()); - } + info_("prompting with : " + request.prompt.prompt, id); return request; } diff --git a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_occupancy_runner.hpp b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_occupancy_runner.hpp index 8a97b1f..2b918aa 100644 --- a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_occupancy_runner.hpp +++ b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_occupancy_runner.hpp @@ -35,15 +35,11 @@ class PromptOccupancyRunner : public ServiceRunner } /** - * @brief Generate a request from parameters given. - * - * This function is used in conjunction with the trigger function to inject type erased parameters - * into the typed action - * - * A pattern needs to be implemented in the derived class - * - * @param parameters - * @return prompt_msgs::srv::Prompt::Request the generated request + * @brief Generate request for the PromptOccupancyRunner + * + * @param parameters parameter values + * @param id thread id + * @return prompt_msgs::srv::Prompt::Request */ virtual typename prompt_msgs::srv::Prompt::Request generate_request(tinyxml2::XMLElement* parameters, int id) override { diff --git a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_runner.hpp b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_runner.hpp index ce88b40..5768d21 100644 --- a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_runner.hpp +++ b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_runner.hpp @@ -49,22 +49,31 @@ class PromptPlanRunner : public ServiceRunner virtual typename prompt_msgs::srv::Prompt::Request generate_request(tinyxml2::XMLElement* parameters, int id) override { bool replan; + const char* task; + std::string taskString; + parameters->QueryBoolAttribute("replan", &replan); + parameters->QueryStringAttribute("task", &task); + + if (task) + taskString = task; + else + taskString = ""; prompt_msgs::srv::Prompt::Request request; if (!replan) { - request.prompt.prompt = "Build a xml plan based on the availbale capabilities to acheive mentioned task. Return " - "only the xml plan without explanations or comments"; + request.prompt.prompt = "Build a xml plan based on the availbale capabilities to acheive mentioned task of " + + taskString + ". Return only the xml plan without explanations or comments"; } else { tinyxml2::XMLElement* failedElements = parameters->FirstChildElement("FailedElements"); - request.prompt.prompt = "Rebuild the xml plan based on the availbale capabilities to acheive mentioned task. " - "Just give the xml plan without explanations or comments. These XML elements had " - "incompatibilities. " + + request.prompt.prompt = "Rebuild the xml plan based on the availbale capabilities to acheive mentioned task of " + + taskString + ". Just give the xml plan without explanations or comments. These XML " + "elements had incompatibilities. " + std::string(failedElements->GetText()) + "Recorrect them as well"; } diff --git a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_pose_runner.hpp b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_pose_runner.hpp index d5dff0a..ff88918 100644 --- a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_pose_runner.hpp +++ b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_pose_runner.hpp @@ -9,10 +9,10 @@ namespace capabilities2_runner { /** - * @brief prompt capability runner + * @brief prompt pose runner * - * This class is a wrapper around the capabilities2 service runner and is used to - * call on the prompt_tools/prompt service, providing it as a capability that prompts + * This class is a wrapper around the capabilities2 service runner and is used to pass + * data to prompt_tools/prompt service, providing it as a capability that prompts * robot pose values */ class PromptPoseRunner : public ServiceRunner @@ -35,17 +35,13 @@ class PromptPoseRunner : public ServiceRunner } /** - * @brief Generate a request from parameters given. - * - * This function is used in conjunction with the trigger function to inject type erased parameters - * into the typed action - * - * A pattern needs to be implemented in the derived class - * - * @param parameters - * @return prompt_msgs::srv::Prompt::Request the generated request + * @brief Generate request for trnasforming data for PrompPoseRunner + * + * @param parameters parameter values + * @param id thread id + * @return prompt_msgs::srv::Prompt::Request */ - virtual typename prompt_msgs::srv::Prompt::Request generate_request(tinyxml2::XMLElement* parameters, int id) override + virtual prompt_msgs::srv::Prompt::Request generate_request(tinyxml2::XMLElement* parameters, int id) override { tinyxml2::XMLElement* poseElement = parameters->FirstChildElement("Pose"); From 52c14dfadf21cc4c36fd4145c3f3889c78bbefbd Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Mon, 10 Feb 2025 17:17:16 +1100 Subject: [PATCH 087/133] completed prompt tools runner implementations --- capabilities2_fabric/plans/default.xml | 8 +- capabilities2_fabric/plans/prompt_1.xml | 2 +- capabilities2_fabric/plans/prompt_2.xml | 2 +- capabilities2_fabric/plans/prompt_3.xml | 2 +- capabilities2_fabric/plans/prompt_4.xml | 4 +- .../capabilities2_runner/service_runner.hpp | 11 +++ .../prompt_capability_runner.hpp | 53 ++-------- .../prompt_occupancy_runner.hpp | 53 +++------- .../prompt_plan_runner.hpp | 64 +++--------- .../prompt_pose_runner.hpp | 54 +++------- .../prompt_service_runner.hpp | 99 +++++++++++++++++++ .../prompt_text_runner.hpp | 52 ++-------- .../src/prompt_runners.cpp | 4 +- 13 files changed, 176 insertions(+), 232 deletions(-) create mode 100644 capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_service_runner.hpp diff --git a/capabilities2_fabric/plans/default.xml b/capabilities2_fabric/plans/default.xml index 038690d..c09a2b0 100644 --- a/capabilities2_fabric/plans/default.xml +++ b/capabilities2_fabric/plans/default.xml @@ -2,12 +2,12 @@ - + - + - - + + \ No newline at end of file diff --git a/capabilities2_fabric/plans/prompt_1.xml b/capabilities2_fabric/plans/prompt_1.xml index 08e399b..b83d8b8 100644 --- a/capabilities2_fabric/plans/prompt_1.xml +++ b/capabilities2_fabric/plans/prompt_1.xml @@ -2,6 +2,6 @@ - + \ No newline at end of file diff --git a/capabilities2_fabric/plans/prompt_2.xml b/capabilities2_fabric/plans/prompt_2.xml index 79a50da..61243ab 100644 --- a/capabilities2_fabric/plans/prompt_2.xml +++ b/capabilities2_fabric/plans/prompt_2.xml @@ -2,6 +2,6 @@ - + \ No newline at end of file diff --git a/capabilities2_fabric/plans/prompt_3.xml b/capabilities2_fabric/plans/prompt_3.xml index 5a81e17..64a54c9 100644 --- a/capabilities2_fabric/plans/prompt_3.xml +++ b/capabilities2_fabric/plans/prompt_3.xml @@ -2,6 +2,6 @@ - + \ No newline at end of file diff --git a/capabilities2_fabric/plans/prompt_4.xml b/capabilities2_fabric/plans/prompt_4.xml index 55d3155..6a51a15 100644 --- a/capabilities2_fabric/plans/prompt_4.xml +++ b/capabilities2_fabric/plans/prompt_4.xml @@ -2,8 +2,8 @@ - - + + \ No newline at end of file diff --git a/capabilities2_runner/include/capabilities2_runner/service_runner.hpp b/capabilities2_runner/include/capabilities2_runner/service_runner.hpp index eda0065..d744cf6 100644 --- a/capabilities2_runner/include/capabilities2_runner/service_runner.hpp +++ b/capabilities2_runner/include/capabilities2_runner/service_runner.hpp @@ -93,6 +93,7 @@ class ServiceRunner : public RunnerBase info_("get result call succeeded", id); response_ = future.get(); + process_response(response_, id); // trigger the events related to on_success state if (events[execute_id].on_success != "") @@ -158,6 +159,16 @@ class ServiceRunner : public RunnerBase */ virtual typename ServiceT::Request generate_request(tinyxml2::XMLElement* parameters, int id) = 0; + /** + * @brief Process the reponse and print data as required + * + * @param response service reponse + * @param id thread id + */ + virtual void process_response(typename ServiceT::Response::SharedPtr response, int id) + { + } + typename rclcpp::Client::SharedPtr service_client_; typename ServiceT::Response::SharedPtr response_; }; diff --git a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_capability_runner.hpp b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_capability_runner.hpp index ab8f43a..65b71d2 100644 --- a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_capability_runner.hpp +++ b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_capability_runner.hpp @@ -2,9 +2,7 @@ #include #include #include -#include -#include -#include +#include namespace capabilities2_runner { @@ -16,36 +14,20 @@ namespace capabilities2_runner * data to prompt_tools/prompt service, providing it as a capability that prompts * robot capabilities. */ -class PromptCapabilityRunner : public ServiceRunner +class PromptCapabilityRunner : public PromptServiceRunner { public: - PromptCapabilityRunner() : ServiceRunner() + PromptCapabilityRunner() : PromptServiceRunner() { } /** - * @brief Starter function for starting the action runner + * @brief generate the prompt used for prompting the capabilities. * - * @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 parameters tinyXML2 parameters + * @return std::string */ - virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config, - std::function print) override - { - init_service(node, run_config, "prompt", print); - } - -protected: - /** - * @brief Generate a request from parameters given. - * - * This function is used in conjunction with the trigger function to inject type erased parameters - * into the typed action - * - * @param parameters - * @return prompt_msgs::srv::Prompt::Request the generated request - */ - virtual prompt_msgs::srv::Prompt::Request generate_request(tinyxml2::XMLElement* parameters, int id) override + virtual std::string generate_prompt(tinyxml2::XMLElement* parameters) { tinyxml2::XMLElement* capabilitySpecsElement = parameters->FirstChildElement("CapabilitySpecs"); @@ -54,26 +36,9 @@ class PromptCapabilityRunner : public ServiceRunner std::string data(printer.CStr()); - prompt_msgs::srv::Prompt::Request request; - - request.prompt.prompt = "The capabilities of the robot are given as follows" + data; - - prompt_msgs::msg::ModelOption modelOption1; - modelOption1.key = "model"; - modelOption1.value = "llama3.1:8b"; - - request.prompt.options.push_back(modelOption1); - - prompt_msgs::msg::ModelOption modelOption2; - modelOption2.key = "stream"; - modelOption2.value = "false"; - modelOption2.type = prompt_msgs::msg::ModelOption::BOOL_TYPE; - - request.prompt.options.push_back(modelOption2); - - info_("prompting with : " + request.prompt.prompt, id); + std::string prompt = "The capabilities of the robot are given as follows" + data; - return request; + return prompt; } }; diff --git a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_occupancy_runner.hpp b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_occupancy_runner.hpp index 2b918aa..52eb134 100644 --- a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_occupancy_runner.hpp +++ b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_occupancy_runner.hpp @@ -2,9 +2,7 @@ #include #include #include -#include -#include -#include +#include namespace capabilities2_runner { @@ -15,33 +13,20 @@ namespace capabilities2_runner * call on the prompt_tools/prompt service, providing it as a capability that prompts * occupancy grid map values */ -class PromptOccupancyRunner : public ServiceRunner +class PromptOccupancyRunner : public PromptServiceRunner { public: - PromptOccupancyRunner() : ServiceRunner() + PromptOccupancyRunner() : PromptServiceRunner() { } /** - * @brief Starter function for starting the service runner + * @brief generate the prompt used for prompting the occupancy grids. * - * @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 parameters tinyXML2 parameters + * @return std::string */ - virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config, - std::function print) override - { - init_service(node, run_config, "prompt", print); - } - - /** - * @brief Generate request for the PromptOccupancyRunner - * - * @param parameters parameter values - * @param id thread id - * @return prompt_msgs::srv::Prompt::Request - */ - virtual typename prompt_msgs::srv::Prompt::Request generate_request(tinyxml2::XMLElement* parameters, int id) override + virtual std::string generate_prompt(tinyxml2::XMLElement* parameters) { tinyxml2::XMLElement* occupancyElement = parameters->FirstChildElement("OccupancyGrid"); @@ -50,27 +35,11 @@ class PromptOccupancyRunner : public ServiceRunner std::string data(printer.CStr()); - prompt_msgs::srv::Prompt::Request request; - - request.prompt.prompt = "The OccupancyGrid of the robot shows the surrounding environment of the robot. The data " - "is given as a ros2 nav_msgs::msg::OccupancyGrid of which the content are " + data; - - prompt_msgs::msg::ModelOption modelOption1; - modelOption1.key = "model"; - modelOption1.value = "llama3.1:8b"; - - request.prompt.options.push_back(modelOption1); - - prompt_msgs::msg::ModelOption modelOption2; - modelOption2.key = "stream"; - modelOption2.value = "false"; - modelOption2.type = prompt_msgs::msg::ModelOption::BOOL_TYPE; - - request.prompt.options.push_back(modelOption2); - - info_("prompting with : " + request.prompt.prompt, id); + std::string prompt = "The OccupancyGrid of the robot shows the surrounding environment of the robot. The data " + "is given as a ros2 nav_msgs::msg::OccupancyGrid of which the content are " + + data; - return request; + return prompt; } }; diff --git a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_runner.hpp b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_runner.hpp index 5768d21..40a15cd 100644 --- a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_runner.hpp +++ b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_runner.hpp @@ -2,9 +2,7 @@ #include #include #include -#include -#include -#include +#include namespace capabilities2_runner { @@ -15,42 +13,25 @@ namespace capabilities2_runner * call on the prompt_tools/prompt service, providing it as a capability that prompts * capabilitie plans values */ -class PromptPlanRunner : public ServiceRunner +class PromptPlanRunner : public PromptServiceRunner { public: - PromptPlanRunner() : ServiceRunner() + PromptPlanRunner() : PromptServiceRunner() { } /** - * @brief Starter function for starting the service runner + * @brief generate the prompt used for prompting the occupancy grids. * - * @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 parameters tinyXML2 parameters + * @return std::string */ - virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config, - std::function print) override - { - init_service(node, run_config, "prompt", print); - } - -protected: - /** - * @brief Generate a request from parameters given. - * - * This function is used in conjunction with the trigger function to inject type erased parameters - * into the typed action - * - * A pattern needs to be implemented in the derived class - * - * @param parameters - * @return prompt_msgs::srv::Prompt::Request the generated request - */ - virtual typename prompt_msgs::srv::Prompt::Request generate_request(tinyxml2::XMLElement* parameters, int id) override + virtual std::string generate_prompt(tinyxml2::XMLElement* parameters) { bool replan; const char* task; std::string taskString; + std::string prompt; parameters->QueryBoolAttribute("replan", &replan); parameters->QueryStringAttribute("task", &task); @@ -60,37 +41,22 @@ class PromptPlanRunner : public ServiceRunner else taskString = ""; - prompt_msgs::srv::Prompt::Request request; - if (!replan) { - request.prompt.prompt = "Build a xml plan based on the availbale capabilities to acheive mentioned task of " + - taskString + ". Return only the xml plan without explanations or comments"; + prompt = "Build a xml plan based on the availbale capabilities to acheive mentioned task of " + taskString + + ". Return only the xml plan without explanations or comments"; } else { tinyxml2::XMLElement* failedElements = parameters->FirstChildElement("FailedElements"); - request.prompt.prompt = "Rebuild the xml plan based on the availbale capabilities to acheive mentioned task of " + - taskString + ". Just give the xml plan without explanations or comments. These XML " - "elements had incompatibilities. " + - std::string(failedElements->GetText()) + "Recorrect them as well"; + prompt = "Rebuild the xml plan based on the availbale capabilities to acheive mentioned task of " + taskString + + ". Just give the xml plan without explanations or comments. These XML " + "elements had incompatibilities. " + + std::string(failedElements->GetText()) + "Recorrect them as well"; } - prompt_msgs::msg::ModelOption modelOption1; - modelOption1.key = "model"; - modelOption1.value = "llama3.1:8b"; - - request.prompt.options.push_back(modelOption1); - - prompt_msgs::msg::ModelOption modelOption2; - modelOption2.key = "stream"; - modelOption2.value = "false"; - modelOption2.type = prompt_msgs::msg::ModelOption::BOOL_TYPE; - - request.prompt.options.push_back(modelOption2); - - return request; + return prompt; } /** diff --git a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_pose_runner.hpp b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_pose_runner.hpp index ff88918..58c3a17 100644 --- a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_pose_runner.hpp +++ b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_pose_runner.hpp @@ -2,9 +2,7 @@ #include #include #include -#include -#include -#include +#include namespace capabilities2_runner { @@ -15,33 +13,20 @@ namespace capabilities2_runner * data to prompt_tools/prompt service, providing it as a capability that prompts * robot pose values */ -class PromptPoseRunner : public ServiceRunner +class PromptPoseRunner : public PromptServiceRunner { public: - PromptPoseRunner() : ServiceRunner() + PromptPoseRunner() : PromptServiceRunner() { } /** - * @brief Starter function for starting the service runner + * @brief generate the prompt used for prompting the capabilities. * - * @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 parameters tinyXML2 parameters + * @return std::string */ - virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config, - std::function print) override - { - init_service(node, run_config, "prompt", print); - } - - /** - * @brief Generate request for trnasforming data for PrompPoseRunner - * - * @param parameters parameter values - * @param id thread id - * @return prompt_msgs::srv::Prompt::Request - */ - virtual prompt_msgs::srv::Prompt::Request generate_request(tinyxml2::XMLElement* parameters, int id) override + virtual std::string generate_prompt(tinyxml2::XMLElement* parameters) { tinyxml2::XMLElement* poseElement = parameters->FirstChildElement("Pose"); @@ -50,27 +35,10 @@ class PromptPoseRunner : public ServiceRunner std::string data(printer.CStr()); - prompt_msgs::srv::Prompt::Request request; - - request.prompt.prompt = "The position of the robot is given as a standard ros2 geometry_msgs::msg::Pose of which " - "the content are " + data; - - prompt_msgs::msg::ModelOption modelOption1; - modelOption1.key = "model"; - modelOption1.value = "llama3.1:8b"; - - request.prompt.options.push_back(modelOption1); - - prompt_msgs::msg::ModelOption modelOption2; - modelOption2.key = "stream"; - modelOption2.value = "false"; - modelOption2.type = prompt_msgs::msg::ModelOption::BOOL_TYPE; - - request.prompt.options.push_back(modelOption2); - - info_("prompting with : " + request.prompt.prompt, id); - - return request; + std::string prompt = "The position of the robot is given as a standard ros2 geometry_msgs::msg::Pose of which " + "the content are " + + data; + return prompt; } }; diff --git a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_service_runner.hpp b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_service_runner.hpp new file mode 100644 index 0000000..50b5c4a --- /dev/null +++ b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_service_runner.hpp @@ -0,0 +1,99 @@ +#pragma once +#include +#include +#include +#include +#include +#include + +namespace capabilities2_runner +{ +/** + * @brief prompt service runner + * + * This class is a base class and a wrapper around the capabilities2 service runner + * and is used to call on the prompt_tools/prompt service, providing it as a capability + * that prompts capabilitie plans values + */ +class PromptServiceRunner : public ServiceRunner +{ +public: + PromptServiceRunner() : ServiceRunner() + { + } + + /** + * @brief Starter function for starting the service runner + * + * @param node shared pointer to the capabilities node. Allows to use ros node related functionalities + * @param run_config runner configuration loaded from the yaml file + */ + virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config, + std::function print) override + { + init_service(node, run_config, "/prompt_tools/prompt", print); + } + +protected: + /** + * @brief Generate a request from parameters given. + * + * This function is used in conjunction with the trigger function to inject type erased parameters + * into the typed action + * + * A pattern needs to be implemented in the derived class + * + * @param parameters + * @return prompt_msgs::srv::Prompt::Request the generated request + */ + virtual typename prompt_msgs::srv::Prompt::Request generate_request(tinyxml2::XMLElement* parameters, int id) override + { + bool stream; + const char* modelChar; + std::string model; + + parameters->QueryBoolAttribute("stream", &stream); + parameters->QueryStringAttribute("model", &modelChar); + + if (modelChar) + model = modelChar; + else + model = "llama3.1:8b"; + + prompt_msgs::srv::Prompt::Request request; + + prompt_msgs::msg::ModelOption modelOption1; + modelOption1.key = "model"; + modelOption1.value = model; + + request.prompt.options.push_back(modelOption1); + + prompt_msgs::msg::ModelOption modelOption2; + modelOption2.key = "stream"; + modelOption2.value = stream; + modelOption2.type = prompt_msgs::msg::ModelOption::BOOL_TYPE; + + request.prompt.options.push_back(modelOption2); + + request.prompt.prompt = generate_prompt(parameters); + + info_("prompting with : " + request.prompt.prompt, id); + + return request; + } + + /** + * @brief generate the prompt used for prompting the data. + * + * @param parameters tinyXML2 parameters + * @return std::string + */ + virtual std::string generate_prompt(tinyxml2::XMLElement* parameters) = 0; + + virtual void process_response(typename prompt_msgs::srv::Prompt::Response::SharedPtr response, int id) + { + info_("response received : " + response->response.response, id); + } +}; + +} // namespace capabilities2_runner diff --git a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_text_runner.hpp b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_text_runner.hpp index 6262cc7..318d797 100644 --- a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_text_runner.hpp +++ b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_text_runner.hpp @@ -2,9 +2,7 @@ #include #include #include -#include -#include -#include +#include namespace capabilities2_runner { @@ -15,37 +13,20 @@ namespace capabilities2_runner * call on the prompt_tools/prompt service, providing it as a capability that prompts * text values */ -class PromptTextRunner : public ServiceRunner +class PromptTextRunner : public PromptServiceRunner { public: - PromptTextRunner() : ServiceRunner() + PromptTextRunner() : PromptServiceRunner() { } /** - * @brief Starter function for starting the service runner + * @brief generate the prompt used for prompting the text. * - * @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 parameters tinyXML2 parameters + * @return std::string */ - virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config, - std::function print) override - { - init_service(node, run_config, "prompt", print); - } - - /** - * @brief Generate a request from parameters given. - * - * This function is used in conjunction with the trigger function to inject type erased parameters - * into the typed action - * - * A pattern needs to be implemented in the derived class - * - * @param parameters - * @return prompt_msgs::srv::Prompt::Request the generated request - */ - virtual typename prompt_msgs::srv::Prompt::Request generate_request(tinyxml2::XMLElement* parameters, int id) override + virtual std::string generate_prompt(tinyxml2::XMLElement* parameters) { tinyxml2::XMLElement* textElement = parameters->FirstChildElement("Text"); @@ -54,24 +35,9 @@ class PromptTextRunner : public ServiceRunner std::string data(printer.CStr()); - prompt_msgs::srv::Prompt::Request request; - - request.prompt.prompt = "The response was " + data; - - prompt_msgs::msg::ModelOption modelOption1; - modelOption1.key = "model"; - modelOption1.value = "llama3.1:8b"; - - request.prompt.options.push_back(modelOption1); - - prompt_msgs::msg::ModelOption modelOption2; - modelOption2.key = "stream"; - modelOption2.value = "false"; - modelOption2.type = prompt_msgs::msg::ModelOption::BOOL_TYPE; - - request.prompt.options.push_back(modelOption2); + std::string prompt = "The response was " + data; - return request; + return prompt; } }; diff --git a/capabilities2_runner_prompt/src/prompt_runners.cpp b/capabilities2_runner_prompt/src/prompt_runners.cpp index 080dda2..1ebcd4d 100644 --- a/capabilities2_runner_prompt/src/prompt_runners.cpp +++ b/capabilities2_runner_prompt/src/prompt_runners.cpp @@ -4,11 +4,11 @@ #include #include #include -#include +#include // register runner plugins PLUGINLIB_EXPORT_CLASS(capabilities2_runner::PromptTextRunner, capabilities2_runner::RunnerBase) PLUGINLIB_EXPORT_CLASS(capabilities2_runner::PromptPoseRunner, capabilities2_runner::RunnerBase) PLUGINLIB_EXPORT_CLASS(capabilities2_runner::PromptOccupancyRunner, capabilities2_runner::RunnerBase) PLUGINLIB_EXPORT_CLASS(capabilities2_runner::PromptPlanRunner, capabilities2_runner::RunnerBase) -PLUGINLIB_EXPORT_CLASS(capabilities2_runner::PromptPlanResponseRunner, capabilities2_runner::RunnerBase) \ No newline at end of file +PLUGINLIB_EXPORT_CLASS(capabilities2_runner::PromptCapabilityRunner, capabilities2_runner::RunnerBase) \ No newline at end of file From 944c90d084bde9056148ecf3773820fee63f7016 Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Mon, 10 Feb 2025 23:33:54 +1100 Subject: [PATCH 088/133] added new examples and sample cases for prompting --- capabilities2_fabric/config/fabric.yaml | 6 ++- .../docs/prompt_capability_runner_ex1.md | 38 ++++++++++++++ .../docs/prompt_occupancy_runner_ex1.md | 52 +++++++++++++++++++ .../docs/prompt_plan_runner_ex1.md | 52 +++++++++++++++++++ .../docs/prompt_pose_runner_ex1.md | 52 +++++++++++++++++++ .../docs/waypoint_runner_ex1.md | 4 +- .../docs/waypoint_runner_ex2.md | 4 +- capabilities2_fabric/readme.md | 23 ++++++-- docs/prompt_tools_setup.md | 18 +++++++ docs/setup.md | 10 ++++ readme.md | 1 + 11 files changed, 251 insertions(+), 9 deletions(-) create mode 100644 capabilities2_fabric/docs/prompt_capability_runner_ex1.md create mode 100644 capabilities2_fabric/docs/prompt_occupancy_runner_ex1.md create mode 100644 capabilities2_fabric/docs/prompt_plan_runner_ex1.md create mode 100644 capabilities2_fabric/docs/prompt_pose_runner_ex1.md create mode 100644 docs/prompt_tools_setup.md diff --git a/capabilities2_fabric/config/fabric.yaml b/capabilities2_fabric/config/fabric.yaml index e12c542..c6a2b99 100644 --- a/capabilities2_fabric/config/fabric.yaml +++ b/capabilities2_fabric/config/fabric.yaml @@ -1,5 +1,9 @@ fabric_client: ros__parameters: - plan_file_path: "install/capabilities2_fabric/share/capabilities2_fabric/plans/navigation_2.xml" # WaypointRunner Example 2 + # plan_file_path: "install/capabilities2_fabric/share/capabilities2_fabric/plans/prompt_4.xml" # PromptPlanRunner Example + # plan_file_path: "install/capabilities2_fabric/share/capabilities2_fabric/plans/prompt_3.xml" # PromptPoseRunner Example + # plan_file_path: "install/capabilities2_fabric/share/capabilities2_fabric/plans/prompt_2.xml" # PromptOccupancyRunner Example + plan_file_path: "install/capabilities2_fabric/share/capabilities2_fabric/plans/prompt_1.xml" # PromptCapabilityRunner Example + # plan_file_path: "install/capabilities2_fabric/share/capabilities2_fabric/plans/navigation_2.xml" # WaypointRunner Example 2 # plan_file_path: "install/capabilities2_fabric/share/capabilities2_fabric/plans/navigation_1.xml" # WaypointRunner Example 1 # plan_file_path: "install/capabilities2_fabric/share/capabilities2_fabric/plans/default.xml" diff --git a/capabilities2_fabric/docs/prompt_capability_runner_ex1.md b/capabilities2_fabric/docs/prompt_capability_runner_ex1.md new file mode 100644 index 0000000..b23e1c7 --- /dev/null +++ b/capabilities2_fabric/docs/prompt_capability_runner_ex1.md @@ -0,0 +1,38 @@ +## PromptCapabilityRunner Example + +### Dependencies + +This example uses prompt tools stack. Follow instructions from [Propmt Tools Dependency Installation](../../docs/prompt_tools_setup.md) to setup Prompt tools stack. + +### Plan selection + +Uncomment the line related to `prompt_1.xml` in the `config/fabric,yaml` file + +### Build the package to apply changes + +In the workspace root run, + +```bash +colcon build +``` + +### Start the Prompt Tools stack + +```bash +source install/setup.bash +ros2 launch prompt_bridge prompt_bridge.launch.py +``` + +### Start the Capabilities2 Server + +```bash +source install/setup.bash +ros2 launch capabilities2_server server.launch.py +``` + +### Start the Capabilities2 Fabric + +```bash +source install/setup.bash +ros2 launch capabilities2_fabric fabric.launch.py +``` \ No newline at end of file diff --git a/capabilities2_fabric/docs/prompt_occupancy_runner_ex1.md b/capabilities2_fabric/docs/prompt_occupancy_runner_ex1.md new file mode 100644 index 0000000..2489180 --- /dev/null +++ b/capabilities2_fabric/docs/prompt_occupancy_runner_ex1.md @@ -0,0 +1,52 @@ +## PromptOccupancyRunner Example + +### Dependencies + +This example uses prompt tools stack, nav2 stack and turtlebot3. Follow instructions from [Nav2 Dependency Installation](../../docs/nav2_setup.md) to setup nav stack and [Propmt Tools Dependency Installation](../../docs/prompt_tools_setup.md) to setup nav stack. + +### Plan selection + +Uncomment the line related to `prompt_2.xml` in the `config/fabric,yaml` file + +### Build the package to apply changes + +In the workspace root run, + +```bash +colcon build +``` + +### Start the turtlebot simulation + +```bash +export TURTLEBOT3_MODEL=waffle +ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py +``` + +### Start the Navigation2 stack + +```bash +source install/setup.bash +ros2 launch nav_stack system.launch.py +``` + +### Start the Prompt Tools stack + +```bash +source install/setup.bash +ros2 launch prompt_bridge prompt_bridge.launch.py +``` + +### Start the Capabilities2 Server + +```bash +source install/setup.bash +ros2 launch capabilities2_server server.launch.py +``` + +### Start the Capabilities2 Fabric + +```bash +source install/setup.bash +ros2 launch capabilities2_fabric fabric.launch.py +``` \ No newline at end of file diff --git a/capabilities2_fabric/docs/prompt_plan_runner_ex1.md b/capabilities2_fabric/docs/prompt_plan_runner_ex1.md new file mode 100644 index 0000000..01ee941 --- /dev/null +++ b/capabilities2_fabric/docs/prompt_plan_runner_ex1.md @@ -0,0 +1,52 @@ +## PromptPlanRunner Example + +### Dependencies + +This example uses prompt tools stack, nav2 stack and turtlebot3. Follow instructions from [Nav2 Dependency Installation](../../docs/nav2_setup.md) to setup nav stack and [Propmt Tools Dependency Installation](../../docs/prompt_tools_setup.md) to setup nav stack. + +### Plan selection + +Uncomment the line related to `prompt_4.xml` in the `config/fabric,yaml` file + +### Build the package to apply changes + +In the workspace root run, + +```bash +colcon build +``` + +### Start the turtlebot simulation + +```bash +export TURTLEBOT3_MODEL=waffle +ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py +``` + +### Start the Navigation2 stack + +```bash +source install/setup.bash +ros2 launch nav_stack system.launch.py +``` + +### Start the Prompt Tools stack + +```bash +source install/setup.bash +ros2 launch prompt_bridge prompt_bridge.launch.py +``` + +### Start the Capabilities2 Server + +```bash +source install/setup.bash +ros2 launch capabilities2_server server.launch.py +``` + +### Start the Capabilities2 Fabric + +```bash +source install/setup.bash +ros2 launch capabilities2_fabric fabric.launch.py +``` \ No newline at end of file diff --git a/capabilities2_fabric/docs/prompt_pose_runner_ex1.md b/capabilities2_fabric/docs/prompt_pose_runner_ex1.md new file mode 100644 index 0000000..7c17a52 --- /dev/null +++ b/capabilities2_fabric/docs/prompt_pose_runner_ex1.md @@ -0,0 +1,52 @@ +## PromptPoseRunner Example + +### Dependencies + +This example uses prompt tools stack, nav2 stack and turtlebot3. Follow instructions from [Nav2 Dependency Installation](../../docs/nav2_setup.md) to setup nav stack and [Propmt Tools Dependency Installation](../../docs/prompt_tools_setup.md) to setup nav stack. + +### Plan selection + +Uncomment the line related to `prompt_3.xml` in the `config/fabric,yaml` file + +### Build the package to apply changes + +In the workspace root run, + +```bash +colcon build +``` + +### Start the turtlebot simulation + +```bash +export TURTLEBOT3_MODEL=waffle +ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py +``` + +### Start the Navigation2 stack + +```bash +source install/setup.bash +ros2 launch nav_stack system.launch.py +``` + +### Start the Prompt Tools stack + +```bash +source install/setup.bash +ros2 launch prompt_bridge prompt_bridge.launch.py +``` + +### Start the Capabilities2 Server + +```bash +source install/setup.bash +ros2 launch capabilities2_server server.launch.py +``` + +### Start the Capabilities2 Fabric + +```bash +source install/setup.bash +ros2 launch capabilities2_fabric fabric.launch.py +``` \ No newline at end of file diff --git a/capabilities2_fabric/docs/waypoint_runner_ex1.md b/capabilities2_fabric/docs/waypoint_runner_ex1.md index 4e978e8..5e199ef 100644 --- a/capabilities2_fabric/docs/waypoint_runner_ex1.md +++ b/capabilities2_fabric/docs/waypoint_runner_ex1.md @@ -1,4 +1,4 @@ -## WaypointRunner Example 1 +## WaypointRunner Example 1 - Single Goal ### Dependencies @@ -37,7 +37,7 @@ source install/setup.bash ros2 launch capabilities2_server server.launch.py ``` -### Start the fabric +### Start the Capabilities2 Fabric ```bash source install/setup.bash diff --git a/capabilities2_fabric/docs/waypoint_runner_ex2.md b/capabilities2_fabric/docs/waypoint_runner_ex2.md index 8a49adb..871b296 100644 --- a/capabilities2_fabric/docs/waypoint_runner_ex2.md +++ b/capabilities2_fabric/docs/waypoint_runner_ex2.md @@ -1,4 +1,4 @@ -## WaypointRunner Example 2 +## WaypointRunner Example 2 - Goal Sequence ### Dependencies @@ -37,7 +37,7 @@ source install/setup.bash ros2 launch capabilities2_server server.launch.py ``` -### Start the fabric +### Start the Capabilities2 Fabric ```bash source install/setup.bash diff --git a/capabilities2_fabric/readme.md b/capabilities2_fabric/readme.md index f541a5a..6217de6 100644 --- a/capabilities2_fabric/readme.md +++ b/capabilities2_fabric/readme.md @@ -51,8 +51,8 @@ Below is an example XML plan for configuring a set of capabilities: - - + + ``` @@ -73,7 +73,22 @@ Below is an example XML plan for configuring a set of capabilities: ### Navigation 1. [WaypointRunner Example 1](./docs/waypoint_runner_ex1.md) -Implements at the very basic fabric triggering that moves the robot from one point to another +Implements at the very basic fabric triggering that moves the robot from one point to another. 2. [WaypointRunner Example 2](./docs/waypoint_runner_ex2.md) -Implements navigating through 2 points using 'sequential' control functionality \ No newline at end of file +Implements navigating through 2 points using 'sequential' control functionality. + + +### Prompting + +1. [PromptCapabilityRunner Example](./docs/prompt_capability_runner_ex1.md) +Implements requesting for robot's capabilities and prompting them to the LLM + +2. [PromptOccupancyRunner Example](./docs/prompt_occupancy_runner_ex1.md) +Implements listening for robot's occupancy grid and prompting them to the LLM + +2. [PromptPoseRunner Example](./docs/prompt_pose_runner_ex1.md) +Implements listening for robot's pose and prompting them to the LLM + +2. [PromptPlanRunner Example](./docs/prompt_plan_runner_ex1.md) +Implements prompting the LLM for a plan for a new task and setting it to Capabilities Fabric \ No newline at end of file diff --git a/docs/prompt_tools_setup.md b/docs/prompt_tools_setup.md new file mode 100644 index 0000000..466c21e --- /dev/null +++ b/docs/prompt_tools_setup.md @@ -0,0 +1,18 @@ +# Dependency installation for Prompt Tools Runners + +## Clone packages + +Clone the prompt tools package same workspace if its not already availabe in the workspace. Capabilities2 Prompt Runners are dependent on this package. + +```bash +cd src +git clone https://github.com/CollaborativeRoboticsLab/prompt_tools.git -b develop +``` + +## Dependency Installation + +Move to workspace root and run the following command to install dependencies + +```bash +rosdep install --from-paths src --ignore-src -r -y +``` \ No newline at end of file diff --git a/docs/setup.md b/docs/setup.md index 736481e..74bce4a 100644 --- a/docs/setup.md +++ b/docs/setup.md @@ -11,6 +11,16 @@ mkdir -p /home/$USER/capabilities_ws/src cd /home/$USER/capabilities_ws/src ``` +### Tinyxml2 Installation + +Clone and install the tinyXML2 package + +```bash +git clone https://github.com/leethomason/tinyxml2.git +cd tinyxml2 +make install +``` + ### Cloning the Packages Clone the package using Git diff --git a/readme.md b/readme.md index a5efcdf..d47f97b 100644 --- a/readme.md +++ b/readme.md @@ -33,6 +33,7 @@ Runners can be created using the runner API parent classes [here](./capabilities - [Setup Instructions with devcontainer](./docs/setup_with_dev.md) - [Setup Instructions without devcontainer](./docs/setup.md) - [Dependency installation for Nav2 Runners](./docs/nav2_setup.md) +- [Dependency installation for Prompt Runners](./docs/prompt_tools_setup.md) ## Quick Startup information From 819bf9089d9a42fd6d54e17c17a6ceedbca9b09b Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Wed, 12 Feb 2025 00:04:48 +1100 Subject: [PATCH 089/133] removed streaming option from prompt_runner commands as prompt_tools does not support that --- capabilities2_fabric/plans/default.xml | 8 ++++---- capabilities2_fabric/plans/prompt_1.xml | 2 +- capabilities2_fabric/plans/prompt_2.xml | 2 +- capabilities2_fabric/plans/prompt_3.xml | 2 +- capabilities2_fabric/plans/prompt_4.xml | 4 ++-- capabilities2_fabric/readme.md | 6 +++--- .../capabilities2_runner_prompt/prompt_service_runner.hpp | 4 +--- 7 files changed, 13 insertions(+), 15 deletions(-) diff --git a/capabilities2_fabric/plans/default.xml b/capabilities2_fabric/plans/default.xml index c09a2b0..12afddf 100644 --- a/capabilities2_fabric/plans/default.xml +++ b/capabilities2_fabric/plans/default.xml @@ -2,12 +2,12 @@ - + - + - - + + \ No newline at end of file diff --git a/capabilities2_fabric/plans/prompt_1.xml b/capabilities2_fabric/plans/prompt_1.xml index b83d8b8..c56b94f 100644 --- a/capabilities2_fabric/plans/prompt_1.xml +++ b/capabilities2_fabric/plans/prompt_1.xml @@ -2,6 +2,6 @@ - + \ No newline at end of file diff --git a/capabilities2_fabric/plans/prompt_2.xml b/capabilities2_fabric/plans/prompt_2.xml index 61243ab..e32c29a 100644 --- a/capabilities2_fabric/plans/prompt_2.xml +++ b/capabilities2_fabric/plans/prompt_2.xml @@ -2,6 +2,6 @@ - + \ No newline at end of file diff --git a/capabilities2_fabric/plans/prompt_3.xml b/capabilities2_fabric/plans/prompt_3.xml index 64a54c9..861b021 100644 --- a/capabilities2_fabric/plans/prompt_3.xml +++ b/capabilities2_fabric/plans/prompt_3.xml @@ -2,6 +2,6 @@ - + \ No newline at end of file diff --git a/capabilities2_fabric/plans/prompt_4.xml b/capabilities2_fabric/plans/prompt_4.xml index 6a51a15..cce166e 100644 --- a/capabilities2_fabric/plans/prompt_4.xml +++ b/capabilities2_fabric/plans/prompt_4.xml @@ -2,8 +2,8 @@ - - + + \ No newline at end of file diff --git a/capabilities2_fabric/readme.md b/capabilities2_fabric/readme.md index 6217de6..e7963a9 100644 --- a/capabilities2_fabric/readme.md +++ b/capabilities2_fabric/readme.md @@ -44,14 +44,14 @@ Below is an example XML plan for configuring a set of capabilities: - + - + - + diff --git a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_service_runner.hpp b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_service_runner.hpp index 50b5c4a..2095251 100644 --- a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_service_runner.hpp +++ b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_service_runner.hpp @@ -48,11 +48,9 @@ class PromptServiceRunner : public ServiceRunner */ virtual typename prompt_msgs::srv::Prompt::Request generate_request(tinyxml2::XMLElement* parameters, int id) override { - bool stream; const char* modelChar; std::string model; - parameters->QueryBoolAttribute("stream", &stream); parameters->QueryStringAttribute("model", &modelChar); if (modelChar) @@ -70,7 +68,7 @@ class PromptServiceRunner : public ServiceRunner prompt_msgs::msg::ModelOption modelOption2; modelOption2.key = "stream"; - modelOption2.value = stream; + modelOption2.value = false; modelOption2.type = prompt_msgs::msg::ModelOption::BOOL_TYPE; request.prompt.options.push_back(modelOption2); From 24d178ee8e009ae9d0c4be36a887c7a86d18056a Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Wed, 12 Feb 2025 14:05:10 +1100 Subject: [PATCH 090/133] updated incorrect plugin.xml files --- capabilities2_runner_capabilities/plugins.xml | 10 +++++++++ capabilities2_runner_nav2/plugins.xml | 8 +++---- .../prompt_service_runner.hpp | 2 +- capabilities2_runner_prompt/plugins.xml | 21 ++++++++++++------- 4 files changed, 28 insertions(+), 13 deletions(-) diff --git a/capabilities2_runner_capabilities/plugins.xml b/capabilities2_runner_capabilities/plugins.xml index 4789f11..32fd1e9 100644 --- a/capabilities2_runner_capabilities/plugins.xml +++ b/capabilities2_runner_capabilities/plugins.xml @@ -1,7 +1,17 @@ + + + A plugin that request capabilities from the capabilities server and transfers to an LLM + + A plugin that notifies about the completion of the fabric to the action server + + + A plugin that sets a new Fabric plan to the Fabric + + diff --git a/capabilities2_runner_nav2/plugins.xml b/capabilities2_runner_nav2/plugins.xml index b7fcdaf..2c4d4ca 100644 --- a/capabilities2_runner_nav2/plugins.xml +++ b/capabilities2_runner_nav2/plugins.xml @@ -1,7 +1,7 @@ - + - A plugin that provide nav2 waypoint navigation capability + A plugin that provide occupancy grid map extraction capability @@ -9,9 +9,9 @@ A plugin that provide robot pose extraction capability - + - A plugin that provide occupancy grid map extraction capability + A plugin that provide nav2 waypoint navigation capability diff --git a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_service_runner.hpp b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_service_runner.hpp index 2095251..1e93f12 100644 --- a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_service_runner.hpp +++ b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_service_runner.hpp @@ -31,7 +31,7 @@ class PromptServiceRunner : public ServiceRunner virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config, std::function print) override { - init_service(node, run_config, "/prompt_tools/prompt", print); + init_service(node, run_config, "/prompt_bridge/prompt", print); } protected: diff --git a/capabilities2_runner_prompt/plugins.xml b/capabilities2_runner_prompt/plugins.xml index f7ea14e..343617d 100644 --- a/capabilities2_runner_prompt/plugins.xml +++ b/capabilities2_runner_prompt/plugins.xml @@ -1,22 +1,27 @@ - + - A plugin that provide text prompting capability + A plugin that provide capability prompting capability - + - A plugin that provide pose prompting capability + A plugin that provide occupancy grid map prompting capability - + - A plugin that provide occupancy grid map prompting capability + A plugin that requests a new pplan based on available capabilities and a defined task - + - A plugin that provide execution plan requesting capability + A plugin that provide pose prompting capability + + + + + A plugin that provide text prompting capability From 10c58097810ad1457fbd81c5917b9d1e8c15e986 Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Sat, 15 Feb 2025 11:44:42 +1100 Subject: [PATCH 091/133] minor update to xml commands --- capabilities2_fabric/plans/default.xml | 8 ++++---- capabilities2_fabric/plans/prompt_1.xml | 2 +- capabilities2_fabric/plans/prompt_2.xml | 2 +- capabilities2_fabric/plans/prompt_3.xml | 2 +- capabilities2_fabric/plans/prompt_4.xml | 4 ++-- capabilities2_fabric/readme.md | 6 +++--- .../prompt_service_runner.hpp | 12 +----------- 7 files changed, 13 insertions(+), 23 deletions(-) diff --git a/capabilities2_fabric/plans/default.xml b/capabilities2_fabric/plans/default.xml index 12afddf..3a9d4b5 100644 --- a/capabilities2_fabric/plans/default.xml +++ b/capabilities2_fabric/plans/default.xml @@ -2,12 +2,12 @@ - + - + - - + + \ No newline at end of file diff --git a/capabilities2_fabric/plans/prompt_1.xml b/capabilities2_fabric/plans/prompt_1.xml index c56b94f..8508f12 100644 --- a/capabilities2_fabric/plans/prompt_1.xml +++ b/capabilities2_fabric/plans/prompt_1.xml @@ -2,6 +2,6 @@ - + \ No newline at end of file diff --git a/capabilities2_fabric/plans/prompt_2.xml b/capabilities2_fabric/plans/prompt_2.xml index e32c29a..1f33259 100644 --- a/capabilities2_fabric/plans/prompt_2.xml +++ b/capabilities2_fabric/plans/prompt_2.xml @@ -2,6 +2,6 @@ - + \ No newline at end of file diff --git a/capabilities2_fabric/plans/prompt_3.xml b/capabilities2_fabric/plans/prompt_3.xml index 861b021..6598ab7 100644 --- a/capabilities2_fabric/plans/prompt_3.xml +++ b/capabilities2_fabric/plans/prompt_3.xml @@ -2,6 +2,6 @@ - + \ No newline at end of file diff --git a/capabilities2_fabric/plans/prompt_4.xml b/capabilities2_fabric/plans/prompt_4.xml index cce166e..b95119b 100644 --- a/capabilities2_fabric/plans/prompt_4.xml +++ b/capabilities2_fabric/plans/prompt_4.xml @@ -2,8 +2,8 @@ - - + + \ No newline at end of file diff --git a/capabilities2_fabric/readme.md b/capabilities2_fabric/readme.md index e7963a9..2e53634 100644 --- a/capabilities2_fabric/readme.md +++ b/capabilities2_fabric/readme.md @@ -44,14 +44,14 @@ Below is an example XML plan for configuring a set of capabilities: - + - + - + diff --git a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_service_runner.hpp b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_service_runner.hpp index 1e93f12..850a9dd 100644 --- a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_service_runner.hpp +++ b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_service_runner.hpp @@ -48,21 +48,11 @@ class PromptServiceRunner : public ServiceRunner */ virtual typename prompt_msgs::srv::Prompt::Request generate_request(tinyxml2::XMLElement* parameters, int id) override { - const char* modelChar; - std::string model; - - parameters->QueryStringAttribute("model", &modelChar); - - if (modelChar) - model = modelChar; - else - model = "llama3.1:8b"; - prompt_msgs::srv::Prompt::Request request; prompt_msgs::msg::ModelOption modelOption1; modelOption1.key = "model"; - modelOption1.value = model; + modelOption1.value = "llama3.1:8b"; request.prompt.options.push_back(modelOption1); From de0ac6c9267beb780e15346e3ca8bf62640e848b Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Mon, 17 Feb 2025 15:04:59 +1100 Subject: [PATCH 092/133] successfully tested prompt_2.xml --- capabilities2_fabric/config/fabric.yaml | 4 ++-- .../prompt_capability_runner.hpp | 2 +- .../prompt_occupancy_runner.hpp | 4 +++- .../capabilities2_runner_prompt/prompt_plan_runner.hpp | 4 +++- .../capabilities2_runner_prompt/prompt_pose_runner.hpp | 9 ++++++--- .../prompt_service_runner.hpp | 8 +++----- .../capabilities2_runner_prompt/prompt_text_runner.hpp | 4 +++- 7 files changed, 21 insertions(+), 14 deletions(-) diff --git a/capabilities2_fabric/config/fabric.yaml b/capabilities2_fabric/config/fabric.yaml index c6a2b99..9ee5bb5 100644 --- a/capabilities2_fabric/config/fabric.yaml +++ b/capabilities2_fabric/config/fabric.yaml @@ -2,8 +2,8 @@ fabric_client: ros__parameters: # plan_file_path: "install/capabilities2_fabric/share/capabilities2_fabric/plans/prompt_4.xml" # PromptPlanRunner Example # plan_file_path: "install/capabilities2_fabric/share/capabilities2_fabric/plans/prompt_3.xml" # PromptPoseRunner Example - # plan_file_path: "install/capabilities2_fabric/share/capabilities2_fabric/plans/prompt_2.xml" # PromptOccupancyRunner Example - plan_file_path: "install/capabilities2_fabric/share/capabilities2_fabric/plans/prompt_1.xml" # PromptCapabilityRunner Example + plan_file_path: "install/capabilities2_fabric/share/capabilities2_fabric/plans/prompt_2.xml" # PromptOccupancyRunner Example + # plan_file_path: "install/capabilities2_fabric/share/capabilities2_fabric/plans/prompt_1.xml" # PromptCapabilityRunner Example # plan_file_path: "install/capabilities2_fabric/share/capabilities2_fabric/plans/navigation_2.xml" # WaypointRunner Example 2 # plan_file_path: "install/capabilities2_fabric/share/capabilities2_fabric/plans/navigation_1.xml" # WaypointRunner Example 1 # plan_file_path: "install/capabilities2_fabric/share/capabilities2_fabric/plans/default.xml" diff --git a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_capability_runner.hpp b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_capability_runner.hpp index 65b71d2..d84e545 100644 --- a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_capability_runner.hpp +++ b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_capability_runner.hpp @@ -27,7 +27,7 @@ class PromptCapabilityRunner : public PromptServiceRunner * @param parameters tinyXML2 parameters * @return std::string */ - virtual std::string generate_prompt(tinyxml2::XMLElement* parameters) + virtual std::string generate_prompt(tinyxml2::XMLElement* parameters, int id) { tinyxml2::XMLElement* capabilitySpecsElement = parameters->FirstChildElement("CapabilitySpecs"); diff --git a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_occupancy_runner.hpp b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_occupancy_runner.hpp index 52eb134..fe6fa29 100644 --- a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_occupancy_runner.hpp +++ b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_occupancy_runner.hpp @@ -26,7 +26,7 @@ class PromptOccupancyRunner : public PromptServiceRunner * @param parameters tinyXML2 parameters * @return std::string */ - virtual std::string generate_prompt(tinyxml2::XMLElement* parameters) + virtual std::string generate_prompt(tinyxml2::XMLElement* parameters, int id) { tinyxml2::XMLElement* occupancyElement = parameters->FirstChildElement("OccupancyGrid"); @@ -39,6 +39,8 @@ class PromptOccupancyRunner : public PromptServiceRunner "is given as a ros2 nav_msgs::msg::OccupancyGrid of which the content are " + data; + info_("prompting with : " + prompt, id); + return prompt; } }; diff --git a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_runner.hpp b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_runner.hpp index 40a15cd..d39f269 100644 --- a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_runner.hpp +++ b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_runner.hpp @@ -26,7 +26,7 @@ class PromptPlanRunner : public PromptServiceRunner * @param parameters tinyXML2 parameters * @return std::string */ - virtual std::string generate_prompt(tinyxml2::XMLElement* parameters) + virtual std::string generate_prompt(tinyxml2::XMLElement* parameters, int id) { bool replan; const char* task; @@ -56,6 +56,8 @@ class PromptPlanRunner : public PromptServiceRunner std::string(failedElements->GetText()) + "Recorrect them as well"; } + info_("prompting with : " + prompt, id); + return prompt; } diff --git a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_pose_runner.hpp b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_pose_runner.hpp index 58c3a17..9ac97bf 100644 --- a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_pose_runner.hpp +++ b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_pose_runner.hpp @@ -26,7 +26,7 @@ class PromptPoseRunner : public PromptServiceRunner * @param parameters tinyXML2 parameters * @return std::string */ - virtual std::string generate_prompt(tinyxml2::XMLElement* parameters) + virtual std::string generate_prompt(tinyxml2::XMLElement* parameters, int id) { tinyxml2::XMLElement* poseElement = parameters->FirstChildElement("Pose"); @@ -36,8 +36,11 @@ class PromptPoseRunner : public PromptServiceRunner std::string data(printer.CStr()); std::string prompt = "The position of the robot is given as a standard ros2 geometry_msgs::msg::Pose of which " - "the content are " + - data; + "the content are " + + data; + + info_("prompting with : " + prompt, id); + return prompt; } }; diff --git a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_service_runner.hpp b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_service_runner.hpp index 850a9dd..44d7bb8 100644 --- a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_service_runner.hpp +++ b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_service_runner.hpp @@ -52,7 +52,7 @@ class PromptServiceRunner : public ServiceRunner prompt_msgs::msg::ModelOption modelOption1; modelOption1.key = "model"; - modelOption1.value = "llama3.1:8b"; + modelOption1.value = "deepseek-r1:32b"; request.prompt.options.push_back(modelOption1); @@ -63,9 +63,7 @@ class PromptServiceRunner : public ServiceRunner request.prompt.options.push_back(modelOption2); - request.prompt.prompt = generate_prompt(parameters); - - info_("prompting with : " + request.prompt.prompt, id); + request.prompt.prompt = generate_prompt(parameters, id); return request; } @@ -76,7 +74,7 @@ class PromptServiceRunner : public ServiceRunner * @param parameters tinyXML2 parameters * @return std::string */ - virtual std::string generate_prompt(tinyxml2::XMLElement* parameters) = 0; + virtual std::string generate_prompt(tinyxml2::XMLElement* parameters, int id) = 0; virtual void process_response(typename prompt_msgs::srv::Prompt::Response::SharedPtr response, int id) { diff --git a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_text_runner.hpp b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_text_runner.hpp index 318d797..970ff3a 100644 --- a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_text_runner.hpp +++ b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_text_runner.hpp @@ -26,7 +26,7 @@ class PromptTextRunner : public PromptServiceRunner * @param parameters tinyXML2 parameters * @return std::string */ - virtual std::string generate_prompt(tinyxml2::XMLElement* parameters) + virtual std::string generate_prompt(tinyxml2::XMLElement* parameters, int id) { tinyxml2::XMLElement* textElement = parameters->FirstChildElement("Text"); @@ -37,6 +37,8 @@ class PromptTextRunner : public PromptServiceRunner std::string prompt = "The response was " + data; + info_("prompting with : " + prompt, id); + return prompt; } }; From dbf7637f18c5330ceb143be42b294b31a409659b Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Tue, 18 Feb 2025 18:19:19 +1100 Subject: [PATCH 093/133] modified topic runner --- capabilities2_fabric/config/fabric.yaml | 4 +- .../capabilities2_runner/topic_runner.hpp | 22 ++- .../occupancygrid_runner.hpp | 138 +++++------------- .../robotpose_runner.hpp | 58 ++------ 4 files changed, 72 insertions(+), 150 deletions(-) diff --git a/capabilities2_fabric/config/fabric.yaml b/capabilities2_fabric/config/fabric.yaml index 9ee5bb5..260f935 100644 --- a/capabilities2_fabric/config/fabric.yaml +++ b/capabilities2_fabric/config/fabric.yaml @@ -1,8 +1,8 @@ fabric_client: ros__parameters: # plan_file_path: "install/capabilities2_fabric/share/capabilities2_fabric/plans/prompt_4.xml" # PromptPlanRunner Example - # plan_file_path: "install/capabilities2_fabric/share/capabilities2_fabric/plans/prompt_3.xml" # PromptPoseRunner Example - plan_file_path: "install/capabilities2_fabric/share/capabilities2_fabric/plans/prompt_2.xml" # PromptOccupancyRunner Example + plan_file_path: "install/capabilities2_fabric/share/capabilities2_fabric/plans/prompt_3.xml" # PromptPoseRunner Example + # plan_file_path: "install/capabilities2_fabric/share/capabilities2_fabric/plans/prompt_2.xml" # PromptOccupancyRunner Example # plan_file_path: "install/capabilities2_fabric/share/capabilities2_fabric/plans/prompt_1.xml" # PromptCapabilityRunner Example # plan_file_path: "install/capabilities2_fabric/share/capabilities2_fabric/plans/navigation_2.xml" # WaypointRunner Example 2 # plan_file_path: "install/capabilities2_fabric/share/capabilities2_fabric/plans/navigation_1.xml" # WaypointRunner Example 1 diff --git a/capabilities2_runner/include/capabilities2_runner/topic_runner.hpp b/capabilities2_runner/include/capabilities2_runner/topic_runner.hpp index 868c401..1d28990 100644 --- a/capabilities2_runner/include/capabilities2_runner/topic_runner.hpp +++ b/capabilities2_runner/include/capabilities2_runner/topic_runner.hpp @@ -40,7 +40,7 @@ class TopicRunner : public RunnerBase // create an service client subscription_ = node_->create_subscription( - topic_name, 1, [this](const typename TopicT::SharedPtr msg) { this->callback(msg); }); + topic_name, 10, [this](const typename TopicT::SharedPtr msg) { this->callback(msg); }); } /** @@ -65,6 +65,15 @@ class TopicRunner : public RunnerBase triggerFunction_(events[execute_id].on_started, update_on_started(events[execute_id].on_started_param)); } + std::unique_lock lock(mutex_); + completed_ = false; + + info_("Waiting for Message.", id); + + // Conditional wait + cv_.wait(lock, [this] { return completed_; }); + info_("Message Received.", id); + if (latest_message_) { // trigger the events related to on_success state @@ -76,7 +85,7 @@ class TopicRunner : public RunnerBase } else { - error_("get result call failed"); + error_("Message receving failed."); // trigger the events related to on_failure state if (events[execute_id].on_failure != "") @@ -85,6 +94,8 @@ class TopicRunner : public RunnerBase triggerFunction_(events[execute_id].on_failure, update_on_failure(events[execute_id].on_failure_param)); } } + + info_("Thread closing.", id); } /** @@ -122,13 +133,16 @@ class TopicRunner : public RunnerBase * * @param msg message parameter */ - void callback(const typename TopicT::SharedPtr& msg) const + void callback(const typename TopicT::SharedPtr& msg) { latest_message_ = msg; + + completed_ = true; + cv_.notify_all(); } typename rclcpp::Subscription::SharedPtr subscription_; - mutable typename TopicT::SharedPtr latest_message_{ nullptr }; + mutable typename TopicT::SharedPtr latest_message_; }; } // namespace capabilities2_runner \ No newline at end of file diff --git a/capabilities2_runner_nav2/include/capabilities2_runner_nav2/occupancygrid_runner.hpp b/capabilities2_runner_nav2/include/capabilities2_runner_nav2/occupancygrid_runner.hpp index 9e658e1..92d17a7 100644 --- a/capabilities2_runner_nav2/include/capabilities2_runner_nav2/occupancygrid_runner.hpp +++ b/capabilities2_runner_nav2/include/capabilities2_runner_nav2/occupancygrid_runner.hpp @@ -32,7 +32,7 @@ class OccupancyGridRunner : public TopicRunner virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config, std::function print) override { - init_subscriber(node, run_config, "map", print); + init_subscriber(node, run_config, "/map", print); } protected: @@ -42,38 +42,19 @@ class OccupancyGridRunner : public TopicRunner * This function is used to inject new data into the XMLElement containing * parameters related to the on_success trigger event * - * - *
- * - * 1700000000 - * 123456789 - * - * map - *
- * - * 0.05 - * 100 - * 100 - * - * - * 1.0 - * 2.0 - * 0.0 - * - * - * 0.0 - * 0.0 - * 0.0 - * 1.0 - * - * - * - * 1700000000 - * 987654321 - * - * - * 0 1 2 3 4 5 6 7 8 9 - * + +
+ +
+ + + + + + + + 0 100 -1 50 25 75 0 0 100 50 + * * @param parameters pointer to the XMLElement containing parameters * @return pointer to the XMLElement containing updated parameters @@ -86,102 +67,61 @@ class OccupancyGridRunner : public TopicRunner tinyxml2::XMLElement* occupancyGridElement = element->GetDocument()->NewElement("OccupancyGrid"); element->InsertEndChild(occupancyGridElement); - // Header element + // Header element with attributes tinyxml2::XMLElement* headerElement = element->GetDocument()->NewElement("header"); - occupancyGridElement->InsertEndChild(headerElement); + headerElement->SetAttribute("frame_id", latest_message_->header.frame_id.c_str()); tinyxml2::XMLElement* stampElement = element->GetDocument()->NewElement("stamp"); + stampElement->SetAttribute("secs", latest_message_->header.stamp.sec); + stampElement->SetAttribute("nsecs", latest_message_->header.stamp.nanosec); headerElement->InsertEndChild(stampElement); - tinyxml2::XMLElement* secsElement = element->GetDocument()->NewElement("secs"); - secsElement->SetText(latest_message_->header.stamp.sec); - stampElement->InsertEndChild(secsElement); - - tinyxml2::XMLElement* nsecsElement = element->GetDocument()->NewElement("nsecs"); - nsecsElement->SetText(latest_message_->header.stamp.nanosec); - stampElement->InsertEndChild(nsecsElement); - - tinyxml2::XMLElement* frameIdElement = element->GetDocument()->NewElement("frame_id"); - frameIdElement->SetText(latest_message_->header.frame_id.c_str()); - headerElement->InsertEndChild(frameIdElement); + occupancyGridElement->InsertEndChild(headerElement); - // Info element + // Info element with attributes tinyxml2::XMLElement* infoElement = element->GetDocument()->NewElement("info"); - occupancyGridElement->InsertEndChild(infoElement); - - tinyxml2::XMLElement* resolutionElement = element->GetDocument()->NewElement("resolution"); - resolutionElement->SetText(latest_message_->info.resolution); - infoElement->InsertEndChild(resolutionElement); + infoElement->SetAttribute("resolution", latest_message_->info.resolution); + infoElement->SetAttribute("width", latest_message_->info.width); + infoElement->SetAttribute("height", latest_message_->info.height); - tinyxml2::XMLElement* widthElement = element->GetDocument()->NewElement("width"); - widthElement->SetText(latest_message_->info.width); - infoElement->InsertEndChild(widthElement); - - tinyxml2::XMLElement* heightElement = element->GetDocument()->NewElement("height"); - heightElement->SetText(latest_message_->info.height); - infoElement->InsertEndChild(heightElement); - - // Origin element + // Origin element with position and orientation attributes tinyxml2::XMLElement* originElement = element->GetDocument()->NewElement("origin"); - infoElement->InsertEndChild(originElement); tinyxml2::XMLElement* positionElement = element->GetDocument()->NewElement("position"); + positionElement->SetAttribute("x", latest_message_->info.origin.position.x); + positionElement->SetAttribute("y", latest_message_->info.origin.position.y); + positionElement->SetAttribute("z", latest_message_->info.origin.position.z); originElement->InsertEndChild(positionElement); - tinyxml2::XMLElement* posX = element->GetDocument()->NewElement("x"); - posX->SetText(latest_message_->info.origin.position.x); - positionElement->InsertEndChild(posX); - - tinyxml2::XMLElement* posY = element->GetDocument()->NewElement("y"); - posY->SetText(latest_message_->info.origin.position.y); - positionElement->InsertEndChild(posY); - - tinyxml2::XMLElement* posZ = element->GetDocument()->NewElement("z"); - posZ->SetText(latest_message_->info.origin.position.z); - positionElement->InsertEndChild(posZ); - tinyxml2::XMLElement* orientationElement = element->GetDocument()->NewElement("orientation"); + orientationElement->SetAttribute("x", latest_message_->info.origin.orientation.x); + orientationElement->SetAttribute("y", latest_message_->info.origin.orientation.y); + orientationElement->SetAttribute("z", latest_message_->info.origin.orientation.z); + orientationElement->SetAttribute("w", latest_message_->info.origin.orientation.w); originElement->InsertEndChild(orientationElement); - tinyxml2::XMLElement* oriX = element->GetDocument()->NewElement("x"); - oriX->SetText(latest_message_->info.origin.orientation.x); - orientationElement->InsertEndChild(oriX); - - tinyxml2::XMLElement* oriY = element->GetDocument()->NewElement("y"); - oriY->SetText(latest_message_->info.origin.orientation.y); - orientationElement->InsertEndChild(oriY); - - tinyxml2::XMLElement* oriZ = element->GetDocument()->NewElement("z"); - oriZ->SetText(latest_message_->info.origin.orientation.z); - orientationElement->InsertEndChild(oriZ); - - tinyxml2::XMLElement* oriW = element->GetDocument()->NewElement("w"); - oriW->SetText(latest_message_->info.origin.orientation.w); - orientationElement->InsertEndChild(oriW); + infoElement->InsertEndChild(originElement); - // Map load time + // Map load time with attributes tinyxml2::XMLElement* mapLoadTimeElement = element->GetDocument()->NewElement("map_load_time"); + mapLoadTimeElement->SetAttribute("secs", latest_message_->info.map_load_time.sec); + mapLoadTimeElement->SetAttribute("nsecs", latest_message_->info.map_load_time.nanosec); infoElement->InsertEndChild(mapLoadTimeElement); - tinyxml2::XMLElement* mapSecsElement = element->GetDocument()->NewElement("secs"); - mapSecsElement->SetText(latest_message_->info.map_load_time.sec); - mapLoadTimeElement->InsertEndChild(mapSecsElement); - - tinyxml2::XMLElement* mapNsecsElement = element->GetDocument()->NewElement("nsecs"); - mapNsecsElement->SetText(latest_message_->info.map_load_time.nanosec); - mapLoadTimeElement->InsertEndChild(mapNsecsElement); + occupancyGridElement->InsertEndChild(infoElement); // Data element for occupancy data (converted to a string) tinyxml2::XMLElement* dataElement = element->GetDocument()->NewElement("data"); - occupancyGridElement->InsertEndChild(dataElement); std::string data_str; - for (size_t i = 0; i < latest_message_->data.size(); ++i) + for (size_t i = 0; i < latest_message_->data.size(); i++) { data_str += std::to_string(latest_message_->data[i]) + " "; } dataElement->SetText(data_str.c_str()); + occupancyGridElement->InsertEndChild(dataElement); + // Return the updated parameters element with OccupancyGrid added std::string result = convert_to_string(element); diff --git a/capabilities2_runner_nav2/include/capabilities2_runner_nav2/robotpose_runner.hpp b/capabilities2_runner_nav2/include/capabilities2_runner_nav2/robotpose_runner.hpp index ed963a3..f05f0ba 100644 --- a/capabilities2_runner_nav2/include/capabilities2_runner_nav2/robotpose_runner.hpp +++ b/capabilities2_runner_nav2/include/capabilities2_runner_nav2/robotpose_runner.hpp @@ -42,19 +42,10 @@ class RobotPoseRunner : public TopicRunner - * - * 1.23 - * 4.56 - * 7.89 - * - * - * 0.12 - * 0.34 - * 0.56 - * 0.78 - * - * + + + + * * @param parameters pointer to the XMLElement containing parameters * @return pointer to the XMLElement containing updated parameters @@ -67,44 +58,21 @@ class RobotPoseRunner : public TopicRunnerGetDocument()->NewElement("Pose"); element->InsertEndChild(poseElement); - // Position element + // Position element with attributes tinyxml2::XMLElement* positionElement = element->GetDocument()->NewElement("position"); + positionElement->SetAttribute("x", latest_message_->pose.pose.position.x); + positionElement->SetAttribute("y", latest_message_->pose.pose.position.y); + positionElement->SetAttribute("z", latest_message_->pose.pose.position.z); poseElement->InsertEndChild(positionElement); - // Position x, y, z - tinyxml2::XMLElement* posX = element->GetDocument()->NewElement("x"); - posX->SetText(latest_message_->pose.pose.position.x); // Set x position value - positionElement->InsertEndChild(posX); - - tinyxml2::XMLElement* posY = element->GetDocument()->NewElement("y"); - posY->SetText(latest_message_->pose.pose.position.y); // Set y position value - positionElement->InsertEndChild(posY); - - tinyxml2::XMLElement* posZ = element->GetDocument()->NewElement("z"); - posZ->SetText(latest_message_->pose.pose.position.z); // Set z position value - positionElement->InsertEndChild(posZ); - - // Orientation element + // Orientation element with attributes tinyxml2::XMLElement* orientationElement = element->GetDocument()->NewElement("orientation"); + orientationElement->SetAttribute("x", latest_message_->pose.pose.orientation.x); + orientationElement->SetAttribute("y", latest_message_->pose.pose.orientation.y); + orientationElement->SetAttribute("z", latest_message_->pose.pose.orientation.z); + orientationElement->SetAttribute("w", latest_message_->pose.pose.orientation.w); poseElement->InsertEndChild(orientationElement); - // Orientation x, y, z, w - tinyxml2::XMLElement* oriX = element->GetDocument()->NewElement("x"); - oriX->SetText(latest_message_->pose.pose.orientation.x); // Set orientation x value - orientationElement->InsertEndChild(oriX); - - tinyxml2::XMLElement* oriY = element->GetDocument()->NewElement("y"); - oriY->SetText(latest_message_->pose.pose.orientation.y); // Set orientation y value - orientationElement->InsertEndChild(oriY); - - tinyxml2::XMLElement* oriZ = element->GetDocument()->NewElement("z"); - oriZ->SetText(latest_message_->pose.pose.orientation.z); // Set orientation z value - orientationElement->InsertEndChild(oriZ); - - tinyxml2::XMLElement* oriW = element->GetDocument()->NewElement("w"); - oriW->SetText(latest_message_->pose.pose.orientation.w); // Set orientation w value - orientationElement->InsertEndChild(oriW); - // Return the updated parameters element with Pose added as string std::string result = convert_to_string(element); From 932fb5188381c10761df260101c5b636c9bc0b6c Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Wed, 19 Feb 2025 02:36:27 +1100 Subject: [PATCH 094/133] added tf2 listnenr instead of subscribing to pose topic in robotpose_runner --- capabilities2_fabric/plans/default.xml | 2 +- capabilities2_fabric/plans/prompt_3.xml | 2 +- capabilities2_fabric/readme.md | 2 +- capabilities2_runner_nav2/CMakeLists.txt | 4 + .../robotpose_runner.hpp | 123 ++++++++++++++++-- 5 files changed, 119 insertions(+), 14 deletions(-) diff --git a/capabilities2_fabric/plans/default.xml b/capabilities2_fabric/plans/default.xml index 3a9d4b5..a49f15b 100644 --- a/capabilities2_fabric/plans/default.xml +++ b/capabilities2_fabric/plans/default.xml @@ -5,7 +5,7 @@ - + diff --git a/capabilities2_fabric/plans/prompt_3.xml b/capabilities2_fabric/plans/prompt_3.xml index 6598ab7..fd491c3 100644 --- a/capabilities2_fabric/plans/prompt_3.xml +++ b/capabilities2_fabric/plans/prompt_3.xml @@ -1,7 +1,7 @@ - + \ No newline at end of file diff --git a/capabilities2_fabric/readme.md b/capabilities2_fabric/readme.md index 2e53634..ced0802 100644 --- a/capabilities2_fabric/readme.md +++ b/capabilities2_fabric/readme.md @@ -47,7 +47,7 @@ Below is an example XML plan for configuring a set of capabilities: - + diff --git a/capabilities2_runner_nav2/CMakeLists.txt b/capabilities2_runner_nav2/CMakeLists.txt index 3edafbc..2c109bd 100644 --- a/capabilities2_runner_nav2/CMakeLists.txt +++ b/capabilities2_runner_nav2/CMakeLists.txt @@ -15,6 +15,8 @@ find_package(rclcpp REQUIRED) find_package(rclcpp_action REQUIRED) find_package(pluginlib REQUIRED) find_package(nav2_msgs REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_ros REQUIRED) find_package(geometry_msgs REQUIRED) find_package(capabilities2_msgs REQUIRED) find_package(capabilities2_runner REQUIRED) @@ -43,6 +45,8 @@ ament_target_dependencies(${PROJECT_NAME} rclcpp_action pluginlib nav2_msgs + tf2 + tf2_ros geometry_msgs capabilities2_msgs capabilities2_runner diff --git a/capabilities2_runner_nav2/include/capabilities2_runner_nav2/robotpose_runner.hpp b/capabilities2_runner_nav2/include/capabilities2_runner_nav2/robotpose_runner.hpp index f05f0ba..aa1347b 100644 --- a/capabilities2_runner_nav2/include/capabilities2_runner_nav2/robotpose_runner.hpp +++ b/capabilities2_runner_nav2/include/capabilities2_runner_nav2/robotpose_runner.hpp @@ -3,10 +3,14 @@ #include #include -#include +#include "geometry_msgs/msg/transform_stamped.hpp" #include +#include "tf2/exceptions.h" +#include "tf2_ros/transform_listener.h" +#include "tf2_ros/buffer.h" + namespace capabilities2_runner { @@ -16,10 +20,10 @@ namespace capabilities2_runner * Capability Class to grab odometry data * */ -class RobotPoseRunner : public TopicRunner +class RobotPoseRunner : public RunnerBase { public: - RobotPoseRunner() : TopicRunner() + RobotPoseRunner() : RunnerBase() { } @@ -32,7 +36,100 @@ class RobotPoseRunner : public TopicRunner runner_publish_func) override { - init_subscriber(node, run_config, "pose", runner_publish_func); + // initialize the runner base by storing node pointer and run config + init_base(node, run_config, runner_publish_func); + + tf_buffer_ = std::make_unique(node->get_clock()); + tf_listener_ = std::make_shared(*tf_buffer_); + } + + /** + * @brief Trigger process to be executed. + * + * This method utilizes paramters set via the trigger() function + * + * @param parameters pointer to tinyxml2::XMLElement that contains parameters + */ + virtual void execution(int id) override + { + const char* from; + const char* to; + + execute_id += 1; + + // if parameters are not provided then cannot proceed + if (!parameters_[id]) + throw runner_exception("cannot grab data without parameters"); + + // trigger the events related to on_started state + if (events[execute_id].on_started != "") + { + info_("on_started", id, events[execute_id].on_started, EventType::STARTED); + triggerFunction_(events[execute_id].on_started, update_on_started(events[execute_id].on_started_param)); + } + + info_("Waiting for Transformation.", id); + + parameters_[id]->QueryStringAttribute("from", &from); + parameters_[id]->QueryStringAttribute("to", &to); + + // Store frame names in variables that will be used to + // compute transformations + std::string fromFrame(from); + std::string toFrame(to); + + // Look up for the transformation between target_frame and turtle2 frames + // and send velocity commands for turtle2 to reach target_frame + try + { + transform_ = tf_buffer_->lookupTransform(toFrame, fromFrame, tf2::TimePointZero); + + // trigger the events related to on_success state + if (events[execute_id].on_success != "") + { + info_("on_success", id, events[execute_id].on_success, EventType::SUCCEEDED); + triggerFunction_(events[execute_id].on_success, update_on_success(events[execute_id].on_success_param)); + } + } + catch (const tf2::TransformException& ex) + { + info_("Could not transform " + toFrame + " to " + fromFrame + " : " + std::string(ex.what()), id); + + // trigger the events related to on_failure state + if (events[execute_id].on_failure != "") + { + info_("on_failure", id, events[execute_id].on_failure, EventType::FAILED); + triggerFunction_(events[execute_id].on_failure, update_on_failure(events[execute_id].on_failure_param)); + } + } + + info_("Thread closing.", id); + } + + /** + * @brief stop function to cease functionality and shutdown + * + */ + virtual void stop() override + { + // if the node pointer is empty then throw an error + // this means that the runner was not started and is being used out of order + + if (!node_) + throw runner_exception("cannot stop runner that was not started"); + + // throw an error if the service client is null + // this can happen if the runner is not able to find the action resource + + if (!tf_listener_) + throw runner_exception("cannot stop runner subscriber that was not started"); + + // Trigger on_stopped event if defined + if (events[execute_id].on_stopped != "") + { + info_("on_stopped", -1, events[execute_id].on_stopped, EventType::STOPPED); + triggerFunction_(events[execute_id].on_stopped, update_on_stopped(events[execute_id].on_stopped_param)); + } } protected: @@ -60,17 +157,17 @@ class RobotPoseRunner : public TopicRunnerGetDocument()->NewElement("position"); - positionElement->SetAttribute("x", latest_message_->pose.pose.position.x); - positionElement->SetAttribute("y", latest_message_->pose.pose.position.y); - positionElement->SetAttribute("z", latest_message_->pose.pose.position.z); + positionElement->SetAttribute("x", transform_.transform.translation.x); + positionElement->SetAttribute("y", transform_.transform.translation.y); + positionElement->SetAttribute("z", transform_.transform.translation.z); poseElement->InsertEndChild(positionElement); // Orientation element with attributes tinyxml2::XMLElement* orientationElement = element->GetDocument()->NewElement("orientation"); - orientationElement->SetAttribute("x", latest_message_->pose.pose.orientation.x); - orientationElement->SetAttribute("y", latest_message_->pose.pose.orientation.y); - orientationElement->SetAttribute("z", latest_message_->pose.pose.orientation.z); - orientationElement->SetAttribute("w", latest_message_->pose.pose.orientation.w); + orientationElement->SetAttribute("x", transform_.transform.rotation.x); + orientationElement->SetAttribute("y", transform_.transform.rotation.y); + orientationElement->SetAttribute("z", transform_.transform.rotation.z); + orientationElement->SetAttribute("w", transform_.transform.rotation.w); poseElement->InsertEndChild(orientationElement); // Return the updated parameters element with Pose added as string @@ -80,5 +177,9 @@ class RobotPoseRunner : public TopicRunner tf_listener_{ nullptr }; + std::unique_ptr tf_buffer_; + geometry_msgs::msg::TransformStamped transform_; }; } // namespace capabilities2_runner \ No newline at end of file From 89e78723b39e8ba989cda3ea5225017e821e326c Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Wed, 19 Feb 2025 16:05:23 +1100 Subject: [PATCH 095/133] tested the robotpose_runner --- capabilities2_fabric/plans/prompt_3.xml | 3 +- .../robotpose_runner.hpp | 52 ++++++++++++++----- 2 files changed, 40 insertions(+), 15 deletions(-) diff --git a/capabilities2_fabric/plans/prompt_3.xml b/capabilities2_fabric/plans/prompt_3.xml index fd491c3..6fc5494 100644 --- a/capabilities2_fabric/plans/prompt_3.xml +++ b/capabilities2_fabric/plans/prompt_3.xml @@ -1,7 +1,8 @@ - + + \ No newline at end of file diff --git a/capabilities2_runner_nav2/include/capabilities2_runner_nav2/robotpose_runner.hpp b/capabilities2_runner_nav2/include/capabilities2_runner_nav2/robotpose_runner.hpp index aa1347b..db90d11 100644 --- a/capabilities2_runner_nav2/include/capabilities2_runner_nav2/robotpose_runner.hpp +++ b/capabilities2_runner_nav2/include/capabilities2_runner_nav2/robotpose_runner.hpp @@ -39,7 +39,7 @@ class RobotPoseRunner : public RunnerBase // initialize the runner base by storing node pointer and run config init_base(node, run_config, runner_publish_func); - tf_buffer_ = std::make_unique(node->get_clock()); + tf_buffer_ = std::make_unique(node_->get_clock()); tf_listener_ = std::make_shared(*tf_buffer_); } @@ -52,8 +52,9 @@ class RobotPoseRunner : public RunnerBase */ virtual void execution(int id) override { - const char* from; - const char* to; + const char* map; + const char* odom; + const char* robot; execute_id += 1; @@ -70,19 +71,20 @@ class RobotPoseRunner : public RunnerBase info_("Waiting for Transformation.", id); - parameters_[id]->QueryStringAttribute("from", &from); - parameters_[id]->QueryStringAttribute("to", &to); + parameters_[id]->QueryStringAttribute("map", &map); + parameters_[id]->QueryStringAttribute("odom", &odom); + parameters_[id]->QueryStringAttribute("robot", &robot); // Store frame names in variables that will be used to // compute transformations - std::string fromFrame(from); - std::string toFrame(to); + std::string mapFrame(map); + std::string odomFrame(odom); + std::string robotFrame(robot); - // Look up for the transformation between target_frame and turtle2 frames - // and send velocity commands for turtle2 to reach target_frame + // Try to use map -> robot first try { - transform_ = tf_buffer_->lookupTransform(toFrame, fromFrame, tf2::TimePointZero); + transform_ = tf_buffer_->lookupTransform(mapFrame, robotFrame, tf2::TimePointZero); // trigger the events related to on_success state if (events[execute_id].on_success != "") @@ -90,10 +92,32 @@ class RobotPoseRunner : public RunnerBase info_("on_success", id, events[execute_id].on_success, EventType::SUCCEEDED); triggerFunction_(events[execute_id].on_success, update_on_success(events[execute_id].on_success_param)); } + + info_("Transformation received. Thread closing.", id); + return; } - catch (const tf2::TransformException& ex) + catch (tf2::TransformException& ex) { - info_("Could not transform " + toFrame + " to " + fromFrame + " : " + std::string(ex.what()), id); + info_("Could not transform from map to robot: " + std::string(ex.what()), id); + } + + // Fall back to odom -> robot + try + { + transform_ = tf_buffer_->lookupTransform(odomFrame, robotFrame, tf2::TimePointZero); + + // trigger the events related to on_success state + if (events[execute_id].on_success != "") + { + info_("on_success", id, events[execute_id].on_success, EventType::SUCCEEDED); + triggerFunction_(events[execute_id].on_success, update_on_success(events[execute_id].on_success_param)); + } + + info_("Transformation received. Thread closing.", id); + } + catch (tf2::TransformException& ex) + { + info_("Could not transform from odom to robot: " + std::string(ex.what()), id); // trigger the events related to on_failure state if (events[execute_id].on_failure != "") @@ -101,9 +125,9 @@ class RobotPoseRunner : public RunnerBase info_("on_failure", id, events[execute_id].on_failure, EventType::FAILED); triggerFunction_(events[execute_id].on_failure, update_on_failure(events[execute_id].on_failure_param)); } - } - info_("Thread closing.", id); + info_("Transformation not received. Thread closing.", id); + } } /** From 306153a0fd6a6b08e355fb254e3c41529bb57a32 Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Tue, 25 Feb 2025 16:24:52 +1100 Subject: [PATCH 096/133] minor changes to plan generation --- capabilities2_fabric/config/fabric.yaml | 4 ++-- .../capabilities2_runner_prompt/prompt_service_runner.hpp | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/capabilities2_fabric/config/fabric.yaml b/capabilities2_fabric/config/fabric.yaml index 260f935..8a94606 100644 --- a/capabilities2_fabric/config/fabric.yaml +++ b/capabilities2_fabric/config/fabric.yaml @@ -1,7 +1,7 @@ fabric_client: ros__parameters: - # plan_file_path: "install/capabilities2_fabric/share/capabilities2_fabric/plans/prompt_4.xml" # PromptPlanRunner Example - plan_file_path: "install/capabilities2_fabric/share/capabilities2_fabric/plans/prompt_3.xml" # PromptPoseRunner Example + plan_file_path: "install/capabilities2_fabric/share/capabilities2_fabric/plans/prompt_4.xml" # PromptPlanRunner Example + # plan_file_path: "install/capabilities2_fabric/share/capabilities2_fabric/plans/prompt_3.xml" # PromptPoseRunner Example # plan_file_path: "install/capabilities2_fabric/share/capabilities2_fabric/plans/prompt_2.xml" # PromptOccupancyRunner Example # plan_file_path: "install/capabilities2_fabric/share/capabilities2_fabric/plans/prompt_1.xml" # PromptCapabilityRunner Example # plan_file_path: "install/capabilities2_fabric/share/capabilities2_fabric/plans/navigation_2.xml" # WaypointRunner Example 2 diff --git a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_service_runner.hpp b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_service_runner.hpp index 44d7bb8..9993123 100644 --- a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_service_runner.hpp +++ b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_service_runner.hpp @@ -52,7 +52,7 @@ class PromptServiceRunner : public ServiceRunner prompt_msgs::msg::ModelOption modelOption1; modelOption1.key = "model"; - modelOption1.value = "deepseek-r1:32b"; + modelOption1.value = "llama3.2"; request.prompt.options.push_back(modelOption1); From 5e83c889e78d8d92fa65c7ec77d6cb3f18d0c568 Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Wed, 26 Feb 2025 00:55:09 +1100 Subject: [PATCH 097/133] updated prompt_runners to match prompt_tools modifications --- .../prompt_capability_runner.hpp | 7 +++---- .../prompt_occupancy_runner.hpp | 11 +++++------ .../prompt_plan_runner.hpp | 8 ++++---- .../prompt_pose_runner.hpp | 10 ++++------ .../prompt_service_runner.hpp | 4 ++-- .../prompt_text_runner.hpp | 7 +++---- 6 files changed, 21 insertions(+), 26 deletions(-) diff --git a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_capability_runner.hpp b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_capability_runner.hpp index d84e545..4bb4673 100644 --- a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_capability_runner.hpp +++ b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_capability_runner.hpp @@ -27,7 +27,7 @@ class PromptCapabilityRunner : public PromptServiceRunner * @param parameters tinyXML2 parameters * @return std::string */ - virtual std::string generate_prompt(tinyxml2::XMLElement* parameters, int id) + virtual void generate_prompt(tinyxml2::XMLElement* parameters, int id, std::string& prompt, bool& flush) override { tinyxml2::XMLElement* capabilitySpecsElement = parameters->FirstChildElement("CapabilitySpecs"); @@ -36,9 +36,8 @@ class PromptCapabilityRunner : public PromptServiceRunner std::string data(printer.CStr()); - std::string prompt = "The capabilities of the robot are given as follows" + data; - - return prompt; + prompt = "The capabilities of the robot are given as follows" + data; + flush = false; } }; diff --git a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_occupancy_runner.hpp b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_occupancy_runner.hpp index fe6fa29..083226f 100644 --- a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_occupancy_runner.hpp +++ b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_occupancy_runner.hpp @@ -26,7 +26,7 @@ class PromptOccupancyRunner : public PromptServiceRunner * @param parameters tinyXML2 parameters * @return std::string */ - virtual std::string generate_prompt(tinyxml2::XMLElement* parameters, int id) + virtual void generate_prompt(tinyxml2::XMLElement* parameters, int id, std::string& prompt, bool& flush) override { tinyxml2::XMLElement* occupancyElement = parameters->FirstChildElement("OccupancyGrid"); @@ -35,13 +35,12 @@ class PromptOccupancyRunner : public PromptServiceRunner std::string data(printer.CStr()); - std::string prompt = "The OccupancyGrid of the robot shows the surrounding environment of the robot. The data " - "is given as a ros2 nav_msgs::msg::OccupancyGrid of which the content are " + - data; + prompt = "The OccupancyGrid of the robot shows the surrounding environment of the robot. The data " + "is given as a ros2 nav_msgs::msg::OccupancyGrid of which the content are " + + data; + flush = false; info_("prompting with : " + prompt, id); - - return prompt; } }; diff --git a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_runner.hpp b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_runner.hpp index d39f269..65b0138 100644 --- a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_runner.hpp +++ b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_runner.hpp @@ -26,12 +26,11 @@ class PromptPlanRunner : public PromptServiceRunner * @param parameters tinyXML2 parameters * @return std::string */ - virtual std::string generate_prompt(tinyxml2::XMLElement* parameters, int id) + virtual void generate_prompt(tinyxml2::XMLElement* parameters, int id, std::string& prompt, bool& flush) override { bool replan; const char* task; std::string taskString; - std::string prompt; parameters->QueryBoolAttribute("replan", &replan); parameters->QueryStringAttribute("task", &task); @@ -45,6 +44,8 @@ class PromptPlanRunner : public PromptServiceRunner { prompt = "Build a xml plan based on the availbale capabilities to acheive mentioned task of " + taskString + ". Return only the xml plan without explanations or comments"; + + flush = true; } else { @@ -54,11 +55,10 @@ class PromptPlanRunner : public PromptServiceRunner ". Just give the xml plan without explanations or comments. These XML " "elements had incompatibilities. " + std::string(failedElements->GetText()) + "Recorrect them as well"; + flush = true; } info_("prompting with : " + prompt, id); - - return prompt; } /** diff --git a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_pose_runner.hpp b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_pose_runner.hpp index 9ac97bf..f2db096 100644 --- a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_pose_runner.hpp +++ b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_pose_runner.hpp @@ -26,7 +26,7 @@ class PromptPoseRunner : public PromptServiceRunner * @param parameters tinyXML2 parameters * @return std::string */ - virtual std::string generate_prompt(tinyxml2::XMLElement* parameters, int id) + virtual void generate_prompt(tinyxml2::XMLElement* parameters, int id, std::string& prompt, bool& flush) override { tinyxml2::XMLElement* poseElement = parameters->FirstChildElement("Pose"); @@ -35,13 +35,11 @@ class PromptPoseRunner : public PromptServiceRunner std::string data(printer.CStr()); - std::string prompt = "The position of the robot is given as a standard ros2 geometry_msgs::msg::Pose of which " - "the content are " + - data; + prompt = "The position of the robot is given as a standard ros2 geometry_msgs::msg::Pose of which the content " + "are " + data; + flush = false; info_("prompting with : " + prompt, id); - - return prompt; } }; diff --git a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_service_runner.hpp b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_service_runner.hpp index 9993123..fd138ad 100644 --- a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_service_runner.hpp +++ b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_service_runner.hpp @@ -63,7 +63,7 @@ class PromptServiceRunner : public ServiceRunner request.prompt.options.push_back(modelOption2); - request.prompt.prompt = generate_prompt(parameters, id); + generate_prompt(parameters, id, request.prompt.prompt, request.prompt.flush); return request; } @@ -74,7 +74,7 @@ class PromptServiceRunner : public ServiceRunner * @param parameters tinyXML2 parameters * @return std::string */ - virtual std::string generate_prompt(tinyxml2::XMLElement* parameters, int id) = 0; + virtual void generate_prompt(tinyxml2::XMLElement* parameters, int id, std::string& prompt, bool& flush) = 0; virtual void process_response(typename prompt_msgs::srv::Prompt::Response::SharedPtr response, int id) { diff --git a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_text_runner.hpp b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_text_runner.hpp index 970ff3a..2a89ab4 100644 --- a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_text_runner.hpp +++ b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_text_runner.hpp @@ -26,7 +26,7 @@ class PromptTextRunner : public PromptServiceRunner * @param parameters tinyXML2 parameters * @return std::string */ - virtual std::string generate_prompt(tinyxml2::XMLElement* parameters, int id) + virtual void generate_prompt(tinyxml2::XMLElement* parameters, int id, std::string& prompt, bool& flush) override { tinyxml2::XMLElement* textElement = parameters->FirstChildElement("Text"); @@ -35,11 +35,10 @@ class PromptTextRunner : public PromptServiceRunner std::string data(printer.CStr()); - std::string prompt = "The response was " + data; + prompt = "The response was " + data; + flush = true; info_("prompting with : " + prompt, id); - - return prompt; } }; From d782d51580b0734b8fe626797d364303de6900d0 Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Wed, 26 Feb 2025 22:52:29 +1100 Subject: [PATCH 098/133] minor modifications --- .../prompt_service_runner.hpp | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_service_runner.hpp b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_service_runner.hpp index fd138ad..d5a7aad 100644 --- a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_service_runner.hpp +++ b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_service_runner.hpp @@ -78,7 +78,14 @@ class PromptServiceRunner : public ServiceRunner virtual void process_response(typename prompt_msgs::srv::Prompt::Response::SharedPtr response, int id) { - info_("response received : " + response->response.response, id); + if (response->response.buffered) + { + info_("information buffered", id); + } + else + { + info_("response received : " + response->response.response, id); + } } }; From d7369ab22651641b56590b205ce86192283cccf0 Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Sun, 9 Mar 2025 21:36:28 +1100 Subject: [PATCH 099/133] completed prompt-testing --- .../capabilities_fabric.hpp | 99 ++++++++++++++----- .../capabilities_fabric_client.hpp | 11 ++- .../utils/bond_client.hpp | 8 +- capabilities2_fabric/plans/prompt_4.xml | 2 +- .../capabilities2_runner/action_runner.hpp | 2 + .../capabilities2_runner/launch_runner.hpp | 2 + .../capabilities2_runner/service_runner.hpp | 2 + .../capabilities2_runner/topic_runner.hpp | 2 + .../prompt_plan_runner.hpp | 2 +- .../capabilities2_server/bond_cache.hpp | 1 + .../capabilities2_server/capabilities_api.hpp | 6 +- .../capabilities_server.hpp | 3 - .../capabilities2_server/runner_cache.hpp | 4 +- 13 files changed, 102 insertions(+), 42 deletions(-) diff --git a/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric.hpp b/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric.hpp index 04c9120..b43f799 100644 --- a/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric.hpp +++ b/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric.hpp @@ -96,6 +96,8 @@ class CapabilitiesFabric : public rclcpp::Node feedback_msg = std::make_shared(); result_msg = std::make_shared(); + + need_reset_ = false; } private: @@ -141,7 +143,10 @@ class CapabilitiesFabric : public rclcpp::Node status_->error("Received the request to cancel the plan"); (void)goal_handle; - bond_client_->stop(); + for (auto& [bond_id, bond_client] : bond_client_cache_) + { + bond_client->stop(); + } return rclcpp_action::CancelResponse::ACCEPT; } @@ -173,6 +178,17 @@ class CapabilitiesFabric : public rclcpp::Node status_->info("Plan after adding closing event :\n\n " + modified_plan); + // if (need_reset_) + // { + // free_capability_all(connection_map); + // need_reset_ = false; + // } + + interface_list.clear(); + providers_list.clear(); + rejected_list.clear(); + connection_map.clear(); + expected_providers_ = 0; completed_providers_ = 0; @@ -181,6 +197,7 @@ class CapabilitiesFabric : public rclcpp::Node expected_capabilities_ = 0; completed_capabilities_ = 0; + freed_capabilities_ = 0; expected_configurations_ = 0; completed_configurations_ = 0; @@ -406,7 +423,7 @@ class CapabilitiesFabric : public rclcpp::Node process_feedback("Connection extraction successful"); // estasblish the bond with the server - establish_bond(); + request_bond(); } /** @@ -419,10 +436,6 @@ class CapabilitiesFabric : public rclcpp::Node auto feedback = std::make_shared(); auto result = std::make_shared(); - // intialize a vector to accomodate elements from both - std::vector tag_list(interface_list.size() + control_tag_list.size()); - std::merge(interface_list.begin(), interface_list.end(), control_tag_list.begin(), control_tag_list.end(), tag_list.begin()); - // verify whether document got 'plan' tags if (!xml_parser::check_plan_tag(document)) { @@ -449,10 +462,10 @@ class CapabilitiesFabric : public rclcpp::Node } /** - * @brief establish the bond with capabilities2 server + * @brief Request the bond from the capabilities2 server * */ - void establish_bond() + void request_bond() { process_feedback("Requesting bond id"); @@ -469,18 +482,40 @@ class CapabilitiesFabric : public rclcpp::Node auto response = future.get(); bond_id_ = response->bond_id; - process_feedback("Received the bond id : " + bond_id_); - bond_client_ = std::make_unique(shared_from_this(), bond_id_); - bond_client_->start(); + establish_bond(); + }); + } - expected_capabilities_ = connection_map.size(); + /** + * @brief establish the bond with capabilities2 server + * + */ + void establish_bond() + { + bond_client_cache_[bond_id_] = std::make_unique(shared_from_this(), bond_id_); + bond_client_cache_[bond_id_]->start(); - process_feedback("Requsting start of " + std::to_string(expected_capabilities_) + " capabilities"); + process_feedback("Bond sucessfully established with bond id : " + bond_id_); - use_capability(connection_map); - }); + if (bond_client_cache_.size() > 1) + { + for (auto& [old_bond_id, bond_client] : bond_client_cache_) + { + if (old_bond_id != bond_id_) + { + bond_client->stop(); + process_feedback("Stopping and removing old bond with id : " + old_bond_id); + } + } + } + + expected_capabilities_ = connection_map.size(); + + process_feedback("Requsting start of " + std::to_string(expected_capabilities_) + " capabilities"); + + use_capability(connection_map); } /** @@ -511,17 +546,17 @@ class CapabilitiesFabric : public rclcpp::Node process_result("Failed to Use capability " + capability + " from " + provider + ". Server Execution Cancelled"); // release all capabilities that were used since not all started successfully - for (const auto& [key, value] : connection_map) + free_capability_all(connection_map); + + for (auto& [bond_id, bond_client] : bond_client_cache_) { - process_feedback("Freeing capability of Node " + std::to_string(key) + " named " + value.source.runner); - free_capability(value.source.runner); + bond_client->stop(); } - - bond_client_->stop(); return; } completed_capabilities_++; + need_reset_ = true; auto response = future.get(); @@ -546,12 +581,14 @@ class CapabilitiesFabric : public rclcpp::Node } /** - * @brief Request use of capability from capabilities2 server + * @brief Free all started capabilities in the capabilities map * - * @param capability capability name to be started + * @param capabilities map of capabilities to be freed */ - void free_capability(const std::string& capability) + void free_capability_all(std::map& capabilities) { + std::string capability = capabilities[freed_capabilities_].source.runner; + auto request_free = std::make_shared(); request_free->capability = capability; request_free->bond_id = bond_id_; @@ -566,6 +603,18 @@ class CapabilitiesFabric : public rclcpp::Node auto response = future.get(); process_feedback("Successfully freed capability " + capability, true); + + freed_capabilities_++; + + // Check if all expected calls are completed before calling verify_plan + if (freed_capabilities_ == completed_capabilities_) + { + process_feedback("All started capabilities have been freed."); + } + else + { + free_capability_all(connection_map); + } }); } @@ -754,6 +803,7 @@ class CapabilitiesFabric : public rclcpp::Node /** flag to select loading from file or accepting via action server */ bool read_file; + bool need_reset_; int expected_interfaces_; int completed_interfaces_; @@ -763,6 +813,7 @@ class CapabilitiesFabric : public rclcpp::Node int expected_capabilities_; int completed_capabilities_; + int freed_capabilities_; int expected_configurations_; int completed_configurations_; @@ -771,7 +822,7 @@ class CapabilitiesFabric : public rclcpp::Node std::string bond_id_; /** Manages bond between capabilities server and this client */ - std::shared_ptr bond_client_; + std::map> bond_client_cache_; /** Handles status message sending and printing to logging */ std::shared_ptr status_; diff --git a/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric_client.hpp b/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric_client.hpp index d5462f8..5ef662d 100644 --- a/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric_client.hpp +++ b/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric_client.hpp @@ -176,11 +176,11 @@ class CapabilitiesFabricClient : public rclcpp::Node if (result.result->success) { - status_->info("Plan executed successfully"); + status_->info("Plan launched successfully"); } else { - status_->error("Plan failed to complete"); + status_->error("Plan failed to launch"); if (result.result->failed_elements.size() > 0) { @@ -199,6 +199,7 @@ class CapabilitiesFabricClient : public rclcpp::Node { if (fabric_state == Status::RUNNING) { + status_->info("Plan canncelling requested"); this->planner_client_->async_cancel_goal(goal_handle_); } @@ -208,7 +209,7 @@ class CapabilitiesFabricClient : public rclcpp::Node void setCompleteCallback(const std::shared_ptr request, std::shared_ptr response) { fabric_state = Status::COMPLETED; - + status_->info("Plan completed successfully"); completed_ = true; cv_.notify_all(); } @@ -267,7 +268,9 @@ class CapabilitiesFabricClient : public rclcpp::Node plan_queue.push_back(request->plan); - if (fabric_state == Status::RUNNING) + status_->info("Plan queued and waiting for execution"); + + if ((fabric_state == Status::RUNNING) or (fabric_state == Status::LAUNCHED)) { status_->info("Prior plan under exeution. Will defer the new plan"); } diff --git a/capabilities2_fabric/include/capabilities2_fabric/utils/bond_client.hpp b/capabilities2_fabric/include/capabilities2_fabric/utils/bond_client.hpp index 401391e..de20de9 100644 --- a/capabilities2_fabric/include/capabilities2_fabric/utils/bond_client.hpp +++ b/capabilities2_fabric/include/capabilities2_fabric/utils/bond_client.hpp @@ -1,4 +1,5 @@ #pragma once +#include #include #include @@ -7,7 +8,7 @@ class BondClient public: BondClient(rclcpp::Node::SharedPtr node, const std::string& bond_id, const std::string& bonds_topic = "/capabilities/bond") { - bonds_topic_ = bonds_topic; + topic_ = bonds_topic; bond_id_ = bond_id; node_ = node; } @@ -16,8 +17,7 @@ class BondClient { RCLCPP_INFO(node_->get_logger(), "[BondClient] creating bond to capabilities server"); - bond_ = - std::make_unique(bonds_topic_, bond_id_, node_, std::bind(&BondClient::on_broken, this), std::bind(&BondClient::on_formed, this)); + bond_ = std::make_unique(topic_, bond_id_, node_, std::bind(&BondClient::on_broken, this), std::bind(&BondClient::on_formed, this)); bond_->setHeartbeatPeriod(0.10); bond_->setHeartbeatTimeout(10.0); @@ -59,7 +59,7 @@ class BondClient std::string bond_id_; /** Bond topic to be published */ - std::string bonds_topic_; + std::string topic_; /** Heart beat bond with capabilities server */ std::shared_ptr bond_; diff --git a/capabilities2_fabric/plans/prompt_4.xml b/capabilities2_fabric/plans/prompt_4.xml index b95119b..dde90ac 100644 --- a/capabilities2_fabric/plans/prompt_4.xml +++ b/capabilities2_fabric/plans/prompt_4.xml @@ -3,7 +3,7 @@ - + \ No newline at end of file diff --git a/capabilities2_runner/include/capabilities2_runner/action_runner.hpp b/capabilities2_runner/include/capabilities2_runner/action_runner.hpp index 43f0fde..1c4b38f 100644 --- a/capabilities2_runner/include/capabilities2_runner/action_runner.hpp +++ b/capabilities2_runner/include/capabilities2_runner/action_runner.hpp @@ -115,6 +115,8 @@ class ActionRunner : public RunnerBase throw runner_exception(e.what()); } } + + info_("stopping runner"); } /** diff --git a/capabilities2_runner/include/capabilities2_runner/launch_runner.hpp b/capabilities2_runner/include/capabilities2_runner/launch_runner.hpp index f2f6ea5..7673f19 100644 --- a/capabilities2_runner/include/capabilities2_runner/launch_runner.hpp +++ b/capabilities2_runner/include/capabilities2_runner/launch_runner.hpp @@ -124,6 +124,8 @@ class LaunchRunner : public RunnerBase RCLCPP_INFO(node_->get_logger(), "Request to launch %s from %s succeeded ", launch_name.c_str(), package_name.c_str()); }); + + info_("stopping runner"); } // throw on trigger function diff --git a/capabilities2_runner/include/capabilities2_runner/service_runner.hpp b/capabilities2_runner/include/capabilities2_runner/service_runner.hpp index d744cf6..0fd505b 100644 --- a/capabilities2_runner/include/capabilities2_runner/service_runner.hpp +++ b/capabilities2_runner/include/capabilities2_runner/service_runner.hpp @@ -143,6 +143,8 @@ class ServiceRunner : public RunnerBase info_("on_stopped", -1, events[execute_id].on_stopped, EventType::STOPPED); triggerFunction_(events[execute_id].on_stopped, update_on_stopped(events[execute_id].on_stopped_param)); } + + info_("stopping runner"); } protected: diff --git a/capabilities2_runner/include/capabilities2_runner/topic_runner.hpp b/capabilities2_runner/include/capabilities2_runner/topic_runner.hpp index 1d28990..bc5001b 100644 --- a/capabilities2_runner/include/capabilities2_runner/topic_runner.hpp +++ b/capabilities2_runner/include/capabilities2_runner/topic_runner.hpp @@ -122,6 +122,8 @@ class TopicRunner : public RunnerBase info_("on_stopped", -1, events[execute_id].on_stopped, EventType::STOPPED); triggerFunction_(events[execute_id].on_stopped, update_on_stopped(events[execute_id].on_stopped_param)); } + + info_("stopping runner"); } protected: diff --git a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_runner.hpp b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_runner.hpp index 65b0138..2cab1eb 100644 --- a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_runner.hpp +++ b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_runner.hpp @@ -43,7 +43,7 @@ class PromptPlanRunner : public PromptServiceRunner if (!replan) { prompt = "Build a xml plan based on the availbale capabilities to acheive mentioned task of " + taskString + - ". Return only the xml plan without explanations or comments"; + ". Return only the xml plan without explanations or comments."; flush = true; } diff --git a/capabilities2_server/include/capabilities2_server/bond_cache.hpp b/capabilities2_server/include/capabilities2_server/bond_cache.hpp index 4895107..4705371 100644 --- a/capabilities2_server/include/capabilities2_server/bond_cache.hpp +++ b/capabilities2_server/include/capabilities2_server/bond_cache.hpp @@ -71,6 +71,7 @@ class BondCache { // remove bond id from capability entry auto it = std::find(bond_cache_[capability].begin(), bond_cache_[capability].end(), bond_id); + if (it != bond_cache_[capability].end()) { bond_cache_[capability].erase(it); diff --git a/capabilities2_server/include/capabilities2_server/capabilities_api.hpp b/capabilities2_server/include/capabilities2_server/capabilities_api.hpp index e95cafb..cc401e2 100644 --- a/capabilities2_server/include/capabilities2_server/capabilities_api.hpp +++ b/capabilities2_server/include/capabilities2_server/capabilities_api.hpp @@ -164,7 +164,7 @@ class CapabilitiesAPI // FIXME: this unrolls the dependency tree from the bottom up but should probably be top down for (const auto& run : running.dependencies) { - print_("freeing dependency: " + run.interface, true, false); + print_("freeing dependency: " + run.interface + "of : " + capability, true, false); // remove the internal 'use' bond for the capability dependency unbind_dependency(run.interface); @@ -180,7 +180,7 @@ class CapabilitiesAPI // this will implicitly stop the runner try { - runner_cache_.remove_runner(capability); + runner_cache_.remove_runner(capability); } catch (const capabilities2_runner::runner_exception& e) { @@ -368,7 +368,7 @@ class CapabilitiesAPI } // couldn't parse unknown capability type - print_("unknown capability type: " +spec.type, true, true); + print_("unknown capability type: " + spec.type, true, true); } // query api diff --git a/capabilities2_server/include/capabilities2_server/capabilities_server.hpp b/capabilities2_server/include/capabilities2_server/capabilities_server.hpp index 1d4187e..c666d15 100644 --- a/capabilities2_server/include/capabilities2_server/capabilities_server.hpp +++ b/capabilities2_server/include/capabilities2_server/capabilities_server.hpp @@ -716,9 +716,6 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI // loop hz double loop_hz_; - /** capabilities_fabric launch thread */ - std::shared_ptr fabric_launch_thread; - // publishers // event publisher rclcpp::Publisher::SharedPtr event_pub_; diff --git a/capabilities2_server/include/capabilities2_server/runner_cache.hpp b/capabilities2_server/include/capabilities2_server/runner_cache.hpp index 784334f..416612c 100644 --- a/capabilities2_server/include/capabilities2_server/runner_cache.hpp +++ b/capabilities2_server/include/capabilities2_server/runner_cache.hpp @@ -184,10 +184,10 @@ class RunnerCache } // reset the runner pointer - runner_cache_[capability].reset(); + // runner_cache_[capability].reset(); // remove the runner from map - runner_cache_.erase(capability); + // runner_cache_.erase(capability); } /** From c0bed5faf2576da4541f1068681e55fbef76e9f4 Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Fri, 28 Mar 2025 23:01:26 +1100 Subject: [PATCH 100/133] Update readme.md --- capabilities2_fabric/readme.md | 32 ++++++++++++++++++++------------ 1 file changed, 20 insertions(+), 12 deletions(-) diff --git a/capabilities2_fabric/readme.md b/capabilities2_fabric/readme.md index ced0802..3bfeb86 100644 --- a/capabilities2_fabric/readme.md +++ b/capabilities2_fabric/readme.md @@ -39,20 +39,28 @@ Below is an example XML plan for configuring a set of capabilities: ```xml - - - - - - + + + + + + + + + + + + + + + - - - + + + - - + ``` @@ -91,4 +99,4 @@ Implements listening for robot's occupancy grid and prompting them to the LLM Implements listening for robot's pose and prompting them to the LLM 2. [PromptPlanRunner Example](./docs/prompt_plan_runner_ex1.md) -Implements prompting the LLM for a plan for a new task and setting it to Capabilities Fabric \ No newline at end of file +Implements prompting the LLM for a plan for a new task and setting it to Capabilities Fabric From 5fa7fd09f3681484f41a3747f221576f168adba6 Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Wed, 23 Apr 2025 23:15:28 +1000 Subject: [PATCH 101/133] added paper citation --- readme.md | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/readme.md b/readme.md index d47f97b..9d0c9d9 100644 --- a/readme.md +++ b/readme.md @@ -68,3 +68,17 @@ This work is based on the capabilities package developed by the Open Source Robo ## Citation If you use this work in an academic context, please cite the following publication(s): + +[Capabilities2 for ROS2: Advanced Skill-Based Control for Human-Robot Interaction](https://dl.acm.org/doi/10.5555/3721488.3721623) +```latex +@inproceedings{10.5555/3721488.3721623, + author = {Pritchard, Michael and Ratnayake, Kalana and Gamage, Buddhi and Jayasuriya, Maleen and Herath, Damith}, + title = {Capabilities2 for ROS2: Advanced Skill-Based Control for Human-Robot Interaction}, + year = {2025}, + publisher = {IEEE Press}, + booktitle = {Proceedings of the 2025 ACM/IEEE International Conference on Human-Robot Interaction}, + pages = {1067–1071}, + location = {Melbourne, Australia}, + series = {HRI '25} +} +``` From 032da526c821d94b4aed08dfdce8c95875d99c09 Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Mon, 28 Apr 2025 18:11:30 +1000 Subject: [PATCH 102/133] resolved tinyxml2 to use ros-provided version using tinyxml2-vendor package --- capabilities2_fabric/CMakeLists.txt | 50 ++++++------------- .../capabilities_fabric_client.hpp | 2 +- .../capabilities2_fabric/utils/xml_parser.hpp | 2 +- capabilities2_fabric/package.xml | 5 +- capabilities2_runner/CMakeLists.txt | 24 +++------ .../capabilities2_runner/action_runner.hpp | 2 +- .../capabilities2_runner/service_runner.hpp | 2 +- .../capabilities2_runner/topic_runner.hpp | 3 +- capabilities2_runner/package.xml | 2 +- capabilities2_runner_audio/CMakeLists.txt | 18 ++----- .../listen_runner.hpp | 2 +- .../speak_runner.hpp | 2 +- capabilities2_runner_audio/package.xml | 3 +- capabilities2_runner_bt/CMakeLists.txt | 18 ++----- capabilities2_runner_bt/package.xml | 2 +- .../CMakeLists.txt | 18 ++----- .../fabric_completion_runner.hpp | 2 +- capabilities2_runner_capabilities/package.xml | 4 +- capabilities2_runner_nav2/CMakeLists.txt | 18 ++----- .../occupancygrid_runner.hpp | 2 +- .../robotpose_runner.hpp | 2 +- capabilities2_runner_nav2/package.xml | 4 +- capabilities2_runner_prompt/CMakeLists.txt | 18 ++----- .../prompt_occupancy_runner.hpp | 2 +- .../prompt_plan_runner.hpp | 2 +- .../prompt_pose_runner.hpp | 2 +- .../prompt_service_runner.hpp | 2 +- .../prompt_text_runner.hpp | 2 +- capabilities2_runner_prompt/package.xml | 4 +- capabilities2_server/CMakeLists.txt | 15 ++---- .../capabilities2_server/capabilities_api.hpp | 2 +- .../capabilities_server.hpp | 2 - capabilities2_server/package.xml | 3 +- 33 files changed, 76 insertions(+), 165 deletions(-) diff --git a/capabilities2_fabric/CMakeLists.txt b/capabilities2_fabric/CMakeLists.txt index 3837283..35d8cae 100644 --- a/capabilities2_fabric/CMakeLists.txt +++ b/capabilities2_fabric/CMakeLists.txt @@ -10,43 +10,35 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() -# find dependencies +# Find dependencies find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_action REQUIRED) +find_package(rclcpp_components REQUIRED) find_package(capabilities2_msgs REQUIRED) +find_package(tinyxml2_vendor REQUIRED) +find_package(TinyXML2 REQUIRED) # provided by tinyxml2 upstream, or tinyxml2_vendor find_package(bondcpp REQUIRED) find_package(backward_ros REQUIRED) -find_package(rclcpp_components REQUIRED) - -# Locate the static version of tinyxml2 -find_library(TINYXML2_LIB NAMES tinyxml2 PATHS /usr/local/lib NO_DEFAULT_PATH) - -if(NOT TINYXML2_LIB) - message(FATAL_ERROR "tinyxml2 library not found. Make sure it's installed as a static library.") -endif() -include_directories(include - /usr/local/include +include_directories( + include ) ############################################################################ -# capabilities2_fabric node implementation that compiles as a executable +# capabilities2_fabric node executable ############################################################################ add_executable(${PROJECT_NAME} src/capabilities_fabric_node.cpp ) -target_link_libraries(${PROJECT_NAME} - ${TINYXML2_LIB} -) - ament_target_dependencies(${PROJECT_NAME} rclcpp rclcpp_action bondcpp capabilities2_msgs + TinyXML2 ) install(TARGETS ${PROJECT_NAME} @@ -54,23 +46,20 @@ install(TARGETS ${PROJECT_NAME} ) ############################################################################ -# capabilities2_fabric component implementation that compiles as a library +# capabilities2_fabric component library ############################################################################ add_library(${PROJECT_NAME}_comp SHARED src/capabilities_fabric_comp.cpp ) -target_link_libraries(${PROJECT_NAME}_comp - ${TINYXML2_LIB} -) - ament_target_dependencies(${PROJECT_NAME}_comp rclcpp rclcpp_action rclcpp_components bondcpp capabilities2_msgs + TinyXML2 ) rclcpp_components_register_node(${PROJECT_NAME}_comp @@ -88,45 +77,39 @@ install(TARGETS ${PROJECT_NAME}_comp ) ############################################################################ -# fabric_client node implementation that compiles as a executable +# fabric_client node executable ############################################################################ add_executable(fabric_client src/capabilities_fabric_client_node.cpp ) -target_link_libraries(fabric_client - ${TINYXML2_LIB} -) - ament_target_dependencies(fabric_client rclcpp rclcpp_action capabilities2_msgs + TinyXML2 ) install(TARGETS fabric_client - DESTINATION lib/${PROJECT_NAME} + DESTINATION lib/${PROJECT_NAME} ) ############################################################################ -# fabric_client component implementation that compiles as a library +# fabric_client component library ############################################################################ add_library(fabric_client_comp SHARED src/capabilities_fabric_client_comp.cpp ) -target_link_libraries(fabric_client_comp - ${TINYXML2_LIB} -) - ament_target_dependencies(fabric_client_comp rclcpp rclcpp_action rclcpp_components bondcpp capabilities2_msgs + TinyXML2 ) rclcpp_components_register_node(fabric_client_comp @@ -144,7 +127,7 @@ install(TARGETS fabric_client_comp ) ############################################################################ -# miscellaneous +# Miscellaneous installations ############################################################################ install(DIRECTORY include/ @@ -163,5 +146,4 @@ install(DIRECTORY plans DESTINATION share/${PROJECT_NAME} ) - ament_package() diff --git a/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric_client.hpp b/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric_client.hpp index 5ef662d..31e65ba 100644 --- a/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric_client.hpp +++ b/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric_client.hpp @@ -4,9 +4,9 @@ #include #include #include -#include #include #include +#include #include #include diff --git a/capabilities2_fabric/include/capabilities2_fabric/utils/xml_parser.hpp b/capabilities2_fabric/include/capabilities2_fabric/utils/xml_parser.hpp index fc4fba1..9defe5a 100644 --- a/capabilities2_fabric/include/capabilities2_fabric/utils/xml_parser.hpp +++ b/capabilities2_fabric/include/capabilities2_fabric/utils/xml_parser.hpp @@ -1,7 +1,7 @@ #pragma once -#include #include #include +#include #include #include #include diff --git a/capabilities2_fabric/package.xml b/capabilities2_fabric/package.xml index 66543ff..0e4bc8f 100644 --- a/capabilities2_fabric/package.xml +++ b/capabilities2_fabric/package.xml @@ -14,10 +14,7 @@ capabilities2_msgs rclcpp_action backward_ros - capabilities2_utils - - - tinyxml2 + tinyxml2_vendor ament_lint_auto ament_lint_common diff --git a/capabilities2_runner/CMakeLists.txt b/capabilities2_runner/CMakeLists.txt index e6bbfe7..8774d9c 100644 --- a/capabilities2_runner/CMakeLists.txt +++ b/capabilities2_runner/CMakeLists.txt @@ -10,31 +10,20 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() +include_directories( + include +) + find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_action REQUIRED) find_package(pluginlib REQUIRED) find_package(capabilities2_msgs REQUIRED) - -# Locate the static version of tinyxml2 -find_library(TINYXML2_LIB NAMES tinyxml2 PATHS /usr/local/lib NO_DEFAULT_PATH) - -if(NOT TINYXML2_LIB) - message(FATAL_ERROR "tinyxml2 library not found. Make sure it's installed as a static library.") -endif() - -include_directories(include - /usr/local/include -) +find_package(tinyxml2_vendor REQUIRED) +find_package(TinyXML2 REQUIRED) # provided by tinyxml2 upstream, or tinyxml2_vendor add_library(${PROJECT_NAME} SHARED src/standard_runners.cpp - # src/launch_runner.cpp - # src/action_runner.cpp -) - -target_link_libraries(${PROJECT_NAME} - ${TINYXML2_LIB} ) ament_target_dependencies(${PROJECT_NAME} @@ -42,6 +31,7 @@ ament_target_dependencies(${PROJECT_NAME} rclcpp_action pluginlib capabilities2_msgs + TinyXML2 ) pluginlib_export_plugin_description_file(${PROJECT_NAME} plugins.xml) diff --git a/capabilities2_runner/include/capabilities2_runner/action_runner.hpp b/capabilities2_runner/include/capabilities2_runner/action_runner.hpp index 1c4b38f..7cb185c 100644 --- a/capabilities2_runner/include/capabilities2_runner/action_runner.hpp +++ b/capabilities2_runner/include/capabilities2_runner/action_runner.hpp @@ -5,8 +5,8 @@ #include #include #include -#include +#include #include #include #include diff --git a/capabilities2_runner/include/capabilities2_runner/service_runner.hpp b/capabilities2_runner/include/capabilities2_runner/service_runner.hpp index 0fd505b..26fe2d4 100644 --- a/capabilities2_runner/include/capabilities2_runner/service_runner.hpp +++ b/capabilities2_runner/include/capabilities2_runner/service_runner.hpp @@ -1,8 +1,8 @@ #pragma once -#include #include "rclcpp/rclcpp.hpp" +#include #include namespace capabilities2_runner diff --git a/capabilities2_runner/include/capabilities2_runner/topic_runner.hpp b/capabilities2_runner/include/capabilities2_runner/topic_runner.hpp index bc5001b..10b49f3 100644 --- a/capabilities2_runner/include/capabilities2_runner/topic_runner.hpp +++ b/capabilities2_runner/include/capabilities2_runner/topic_runner.hpp @@ -1,9 +1,8 @@ #pragma once -#include #include #include "rclcpp/rclcpp.hpp" - +#include #include namespace capabilities2_runner diff --git a/capabilities2_runner/package.xml b/capabilities2_runner/package.xml index e010e22..117ddcf 100644 --- a/capabilities2_runner/package.xml +++ b/capabilities2_runner/package.xml @@ -20,10 +20,10 @@ pluginlib std_msgs capabilities2_msgs + tinyxml2_vendor uuid - tinyxml2 ament_cmake diff --git a/capabilities2_runner_audio/CMakeLists.txt b/capabilities2_runner_audio/CMakeLists.txt index 9c39a62..1e3797a 100644 --- a/capabilities2_runner_audio/CMakeLists.txt +++ b/capabilities2_runner_audio/CMakeLists.txt @@ -18,26 +18,17 @@ find_package(pluginlib REQUIRED) find_package(hri_audio_msgs REQUIRED) find_package(capabilities2_msgs REQUIRED) find_package(capabilities2_runner REQUIRED) +find_package(tinyxml2_vendor REQUIRED) +find_package(TinyXML2 REQUIRED) # provided by tinyxml2 upstream, or tinyxml2_vendor -# Locate the static version of tinyxml2 -find_library(TINYXML2_LIB NAMES tinyxml2 PATHS /usr/local/lib NO_DEFAULT_PATH) - -if(NOT TINYXML2_LIB) - message(FATAL_ERROR "tinyxml2 library not found. Make sure it's installed as a static library.") -endif() - -include_directories(include - /usr/local/include +include_directories( + include ) add_library(${PROJECT_NAME} SHARED src/audio_runners.cpp ) -target_link_libraries(${PROJECT_NAME} - ${TINYXML2_LIB} -) - ament_target_dependencies(${PROJECT_NAME} rclcpp rclcpp_action @@ -45,6 +36,7 @@ ament_target_dependencies(${PROJECT_NAME} hri_audio_msgs capabilities2_msgs capabilities2_runner + TinyXML2 ) pluginlib_export_plugin_description_file(capabilities2_runner plugins.xml) 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 62607a1..1985914 100644 --- a/capabilities2_runner_audio/include/capabilities2_runner_audio/listen_runner.hpp +++ b/capabilities2_runner_audio/include/capabilities2_runner_audio/listen_runner.hpp @@ -1,8 +1,8 @@ #pragma once #include -#include +#include #include #include diff --git a/capabilities2_runner_audio/include/capabilities2_runner_audio/speak_runner.hpp b/capabilities2_runner_audio/include/capabilities2_runner_audio/speak_runner.hpp index f4fd976..a776f63 100644 --- a/capabilities2_runner_audio/include/capabilities2_runner_audio/speak_runner.hpp +++ b/capabilities2_runner_audio/include/capabilities2_runner_audio/speak_runner.hpp @@ -1,8 +1,8 @@ #pragma once #include -#include +#include #include #include diff --git a/capabilities2_runner_audio/package.xml b/capabilities2_runner_audio/package.xml index 1d45b25..970a446 100644 --- a/capabilities2_runner_audio/package.xml +++ b/capabilities2_runner_audio/package.xml @@ -17,10 +17,9 @@ hri_audio_msgs capabilities2_msgs capabilities2_runner + tinyxml2_vendor - tinyxml2 - ros2launch ament_lint_auto diff --git a/capabilities2_runner_bt/CMakeLists.txt b/capabilities2_runner_bt/CMakeLists.txt index bcae60c..1d6386e 100644 --- a/capabilities2_runner_bt/CMakeLists.txt +++ b/capabilities2_runner_bt/CMakeLists.txt @@ -17,26 +17,17 @@ find_package(pluginlib REQUIRED) find_package(capabilities2_msgs REQUIRED) find_package(capabilities2_runner REQUIRED) find_package(behaviortree_cpp REQUIRED) +find_package(tinyxml2_vendor REQUIRED) +find_package(TinyXML2 REQUIRED) # provided by tinyxml2 upstream, or tinyxml2_vendor -# Locate the static version of tinyxml2 -find_library(TINYXML2_LIB NAMES tinyxml2 PATHS /usr/local/lib NO_DEFAULT_PATH) - -if(NOT TINYXML2_LIB) - message(FATAL_ERROR "tinyxml2 library not found. Make sure it's installed as a static library.") -endif() - -include_directories(include - /usr/local/include +include_directories( + include ) add_library(${PROJECT_NAME} SHARED src/bt_runners.cpp ) -target_link_libraries(${PROJECT_NAME} - ${TINYXML2_LIB} -) - ament_target_dependencies(${PROJECT_NAME} rclcpp rclcpp_action @@ -44,6 +35,7 @@ ament_target_dependencies(${PROJECT_NAME} capabilities2_msgs capabilities2_runner behaviortree_cpp + TinyXML2 ) pluginlib_export_plugin_description_file(capabilities2_runner plugins.xml) diff --git a/capabilities2_runner_bt/package.xml b/capabilities2_runner_bt/package.xml index 2c84b98..1be5306 100644 --- a/capabilities2_runner_bt/package.xml +++ b/capabilities2_runner_bt/package.xml @@ -21,11 +21,11 @@ std_msgs capabilities2_msgs capabilities2_runner + tinyxml2_vendor behaviortree_cpp uuid - tinyxml2 ament_cmake diff --git a/capabilities2_runner_capabilities/CMakeLists.txt b/capabilities2_runner_capabilities/CMakeLists.txt index d72bd90..9e99551 100644 --- a/capabilities2_runner_capabilities/CMakeLists.txt +++ b/capabilities2_runner_capabilities/CMakeLists.txt @@ -16,32 +16,24 @@ find_package(rclcpp_action REQUIRED) find_package(pluginlib REQUIRED) find_package(capabilities2_msgs REQUIRED) find_package(capabilities2_runner REQUIRED) +find_package(tinyxml2_vendor REQUIRED) +find_package(TinyXML2 REQUIRED) # provided by tinyxml2 upstream, or tinyxml2_vendor -# Locate the static version of tinyxml2 -find_library(TINYXML2_LIB NAMES tinyxml2 PATHS /usr/local/lib NO_DEFAULT_PATH) - -if(NOT TINYXML2_LIB) - message(FATAL_ERROR "tinyxml2 library not found. Make sure it's installed as a static library.") -endif() - -include_directories(include - /usr/local/include +include_directories( + include ) add_library(${PROJECT_NAME} SHARED src/capabilities2_runner.cpp ) -target_link_libraries(${PROJECT_NAME} - ${TINYXML2_LIB} -) - ament_target_dependencies(${PROJECT_NAME} rclcpp rclcpp_action pluginlib capabilities2_msgs capabilities2_runner + TinyXML2 ) pluginlib_export_plugin_description_file(capabilities2_runner plugins.xml) diff --git a/capabilities2_runner_capabilities/include/capabilities2_runner_capabilities/fabric_completion_runner.hpp b/capabilities2_runner_capabilities/include/capabilities2_runner_capabilities/fabric_completion_runner.hpp index 828fb7c..d849dbf 100644 --- a/capabilities2_runner_capabilities/include/capabilities2_runner_capabilities/fabric_completion_runner.hpp +++ b/capabilities2_runner_capabilities/include/capabilities2_runner_capabilities/fabric_completion_runner.hpp @@ -1,6 +1,6 @@ #pragma once -#include #include +#include #include #include #include diff --git a/capabilities2_runner_capabilities/package.xml b/capabilities2_runner_capabilities/package.xml index 93ddac1..7546f41 100644 --- a/capabilities2_runner_capabilities/package.xml +++ b/capabilities2_runner_capabilities/package.xml @@ -18,10 +18,8 @@ std_msgs capabilities2_msgs capabilities2_runner + tinyxml2_vendor - - tinyxml2 - ament_cmake diff --git a/capabilities2_runner_nav2/CMakeLists.txt b/capabilities2_runner_nav2/CMakeLists.txt index 2c109bd..6194b9f 100644 --- a/capabilities2_runner_nav2/CMakeLists.txt +++ b/capabilities2_runner_nav2/CMakeLists.txt @@ -20,26 +20,17 @@ find_package(tf2_ros REQUIRED) find_package(geometry_msgs REQUIRED) find_package(capabilities2_msgs REQUIRED) find_package(capabilities2_runner REQUIRED) +find_package(tinyxml2_vendor REQUIRED) +find_package(TinyXML2 REQUIRED) # provided by tinyxml2 upstream, or tinyxml2_vendor -# Locate the static version of tinyxml2 -find_library(TINYXML2_LIB NAMES tinyxml2 PATHS /usr/local/lib NO_DEFAULT_PATH) - -if(NOT TINYXML2_LIB) - message(FATAL_ERROR "tinyxml2 library not found. Make sure it's installed as a static library.") -endif() - -include_directories(include - /usr/local/include +include_directories( + include ) add_library(${PROJECT_NAME} SHARED src/nav2_runners.cpp ) -target_link_libraries(${PROJECT_NAME} - ${TINYXML2_LIB} -) - ament_target_dependencies(${PROJECT_NAME} rclcpp rclcpp_action @@ -50,6 +41,7 @@ ament_target_dependencies(${PROJECT_NAME} geometry_msgs capabilities2_msgs capabilities2_runner + TinyXML2 ) pluginlib_export_plugin_description_file(capabilities2_runner plugins.xml) diff --git a/capabilities2_runner_nav2/include/capabilities2_runner_nav2/occupancygrid_runner.hpp b/capabilities2_runner_nav2/include/capabilities2_runner_nav2/occupancygrid_runner.hpp index 92d17a7..ede210c 100644 --- a/capabilities2_runner_nav2/include/capabilities2_runner_nav2/occupancygrid_runner.hpp +++ b/capabilities2_runner_nav2/include/capabilities2_runner_nav2/occupancygrid_runner.hpp @@ -1,8 +1,8 @@ #pragma once -#include #include +#include #include #include diff --git a/capabilities2_runner_nav2/include/capabilities2_runner_nav2/robotpose_runner.hpp b/capabilities2_runner_nav2/include/capabilities2_runner_nav2/robotpose_runner.hpp index db90d11..298949d 100644 --- a/capabilities2_runner_nav2/include/capabilities2_runner_nav2/robotpose_runner.hpp +++ b/capabilities2_runner_nav2/include/capabilities2_runner_nav2/robotpose_runner.hpp @@ -1,8 +1,8 @@ #pragma once -#include #include +#include #include "geometry_msgs/msg/transform_stamped.hpp" #include diff --git a/capabilities2_runner_nav2/package.xml b/capabilities2_runner_nav2/package.xml index 4f32f58..305326a 100644 --- a/capabilities2_runner_nav2/package.xml +++ b/capabilities2_runner_nav2/package.xml @@ -16,9 +16,7 @@ geometry_msgs capabilities2_msgs capabilities2_runner - - - tinyxml2 + tinyxml2_vendor ament_lint_auto ament_lint_common diff --git a/capabilities2_runner_prompt/CMakeLists.txt b/capabilities2_runner_prompt/CMakeLists.txt index ab76761..ca72255 100644 --- a/capabilities2_runner_prompt/CMakeLists.txt +++ b/capabilities2_runner_prompt/CMakeLists.txt @@ -17,26 +17,17 @@ find_package(pluginlib REQUIRED) find_package(prompt_msgs REQUIRED) find_package(capabilities2_msgs REQUIRED) find_package(capabilities2_runner REQUIRED) +find_package(tinyxml2_vendor REQUIRED) +find_package(TinyXML2 REQUIRED) # provided by tinyxml2 upstream, or tinyxml2_vendor -# Locate the static version of tinyxml2 -find_library(TINYXML2_LIB NAMES tinyxml2 PATHS /usr/local/lib NO_DEFAULT_PATH) - -if(NOT TINYXML2_LIB) - message(FATAL_ERROR "tinyxml2 library not found. Make sure it's installed as a static library.") -endif() - -include_directories(include - /usr/local/include +include_directories( + include ) add_library(${PROJECT_NAME} SHARED src/prompt_runners.cpp ) -target_link_libraries(${PROJECT_NAME} - ${TINYXML2_LIB} -) - ament_target_dependencies(${PROJECT_NAME} rclcpp rclcpp_action @@ -44,6 +35,7 @@ ament_target_dependencies(${PROJECT_NAME} prompt_msgs capabilities2_msgs capabilities2_runner + TinyXML2 ) pluginlib_export_plugin_description_file(capabilities2_runner plugins.xml) diff --git a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_occupancy_runner.hpp b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_occupancy_runner.hpp index 083226f..529773c 100644 --- a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_occupancy_runner.hpp +++ b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_occupancy_runner.hpp @@ -1,6 +1,6 @@ #pragma once -#include #include +#include #include #include diff --git a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_runner.hpp b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_runner.hpp index 2cab1eb..e9b3460 100644 --- a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_runner.hpp +++ b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_runner.hpp @@ -1,6 +1,6 @@ #pragma once -#include #include +#include #include #include diff --git a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_pose_runner.hpp b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_pose_runner.hpp index f2db096..4c831d9 100644 --- a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_pose_runner.hpp +++ b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_pose_runner.hpp @@ -1,6 +1,6 @@ #pragma once -#include #include +#include #include #include diff --git a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_service_runner.hpp b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_service_runner.hpp index d5a7aad..5ec9959 100644 --- a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_service_runner.hpp +++ b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_service_runner.hpp @@ -1,6 +1,6 @@ #pragma once -#include #include +#include #include #include #include diff --git a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_text_runner.hpp b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_text_runner.hpp index 2a89ab4..026f70b 100644 --- a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_text_runner.hpp +++ b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_text_runner.hpp @@ -1,6 +1,6 @@ #pragma once -#include #include +#include #include #include diff --git a/capabilities2_runner_prompt/package.xml b/capabilities2_runner_prompt/package.xml index 21697db..0f56363 100644 --- a/capabilities2_runner_prompt/package.xml +++ b/capabilities2_runner_prompt/package.xml @@ -15,9 +15,7 @@ prompt_msgs capabilities2_msgs capabilities2_runner - - - tinyxml2 + tinyxml2_vendor ament_lint_auto ament_lint_common diff --git a/capabilities2_server/CMakeLists.txt b/capabilities2_server/CMakeLists.txt index aa21c61..1e7e4c0 100644 --- a/capabilities2_server/CMakeLists.txt +++ b/capabilities2_server/CMakeLists.txt @@ -19,18 +19,13 @@ find_package(pluginlib REQUIRED) find_package(rclcpp_components REQUIRED) find_package(capabilities2_msgs REQUIRED) find_package(capabilities2_runner REQUIRED) +find_package(tinyxml2_vendor REQUIRED) +find_package(TinyXML2 REQUIRED) # provided by tinyxml2 upstream, or tinyxml2_vendor find_package(backward_ros REQUIRED) find_package(PkgConfig) pkg_check_modules(UUID REQUIRED uuid) -# Locate the static version of tinyxml2 -find_library(TINYXML2_LIB NAMES tinyxml2 PATHS /usr/local/lib NO_DEFAULT_PATH) - -if(NOT TINYXML2_LIB) - message(FATAL_ERROR "tinyxml2 library not found. Make sure it's installed as a static library.") -endif() - # Find SQLite3 find_package(SQLite3) @@ -40,7 +35,6 @@ find_package(yaml-cpp REQUIRED) include_directories(include ${SQLITE3_INCLUDE_DIRS} ${YAML_CPP_INCLUDE_DIRS} - /usr/local/include ${UUID_INCLUDE_DIRS} ) @@ -55,7 +49,6 @@ add_library(${PROJECT_NAME} SHARED target_link_libraries(${PROJECT_NAME} ${SQLITE3_LIBRARIES} ${YAML_CPP_LIBRARIES} - ${TINYXML2_LIB} ${UUID_LIBRARIES} ) @@ -67,6 +60,7 @@ ament_target_dependencies(${PROJECT_NAME} rclcpp_components capabilities2_msgs capabilities2_runner + TinyXML2 SQLite3 yaml-cpp UUID @@ -96,7 +90,6 @@ add_executable(${PROJECT_NAME}_node target_link_libraries(${PROJECT_NAME}_node ${UUID_LIBRARIES} - ${TINYXML2_LIB} ${SQLITE3_LIBRARIES} ${YAML_CPP_LIBRARIES} ) @@ -109,6 +102,7 @@ ament_target_dependencies(${PROJECT_NAME}_node rclcpp_components capabilities2_msgs capabilities2_runner + TinyXML2 SQLite3 yaml-cpp UUID @@ -125,7 +119,6 @@ target_link_libraries(test_capabilities_server ${PROJECT_NAME} ${SQLITE3_LIBRARIES} ${YAML_CPP_LIBRARIES} - ${TINYXML2_LIB} ${UUID_LIBRARIES} ) add_dependencies(test_capabilities_server ${PROJECT_NAME}) diff --git a/capabilities2_server/include/capabilities2_server/capabilities_api.hpp b/capabilities2_server/include/capabilities2_server/capabilities_api.hpp index cc401e2..291c211 100644 --- a/capabilities2_server/include/capabilities2_server/capabilities_api.hpp +++ b/capabilities2_server/include/capabilities2_server/capabilities_api.hpp @@ -6,9 +6,9 @@ #include #include +#include #include #include -#include #include diff --git a/capabilities2_server/include/capabilities2_server/capabilities_server.hpp b/capabilities2_server/include/capabilities2_server/capabilities_server.hpp index c666d15..6746c65 100644 --- a/capabilities2_server/include/capabilities2_server/capabilities_server.hpp +++ b/capabilities2_server/include/capabilities2_server/capabilities_server.hpp @@ -7,9 +7,7 @@ #include #include #include - #include - #include #include diff --git a/capabilities2_server/package.xml b/capabilities2_server/package.xml index 95062d6..8d70790 100644 --- a/capabilities2_server/package.xml +++ b/capabilities2_server/package.xml @@ -22,11 +22,10 @@ rclcpp_components capabilities2_msgs capabilities2_runner - capabilities2_utils + tinyxml2_vendor yaml-cpp - tinyxml2 libsqlite3-dev uuid backward_ros From 973efc24ccc4d1082c106ff56e64d6142fd0ec91 Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Wed, 30 Apr 2025 00:54:38 +1000 Subject: [PATCH 103/133] moved event publishing related functionality to a new package and a header based client --- capabilities2_events/.clang-format | 64 +++++ capabilities2_events/CMakeLists.txt | 17 ++ .../capabilities2_events/event_client.hpp | 151 ++++++++++++ capabilities2_events/package.xml | 21 ++ capabilities2_fabric/CMakeLists.txt | 5 + .../capabilities_fabric.hpp | 229 +++++++++--------- .../capabilities_fabric_client.hpp | 74 +++--- .../utils/status_client.hpp | 110 --------- .../capabilities2_fabric/utils/xml_parser.hpp | 18 +- capabilities2_fabric/package.xml | 3 +- capabilities2_msgs/CMakeLists.txt | 1 - capabilities2_msgs/msg/CapabilityEvent.msg | 30 +-- capabilities2_msgs/msg/Status.msg | 7 - 13 files changed, 426 insertions(+), 304 deletions(-) create mode 100644 capabilities2_events/.clang-format create mode 100644 capabilities2_events/CMakeLists.txt create mode 100644 capabilities2_events/include/capabilities2_events/event_client.hpp create mode 100644 capabilities2_events/package.xml delete mode 100644 capabilities2_fabric/include/capabilities2_fabric/utils/status_client.hpp delete mode 100644 capabilities2_msgs/msg/Status.msg diff --git a/capabilities2_events/.clang-format b/capabilities2_events/.clang-format new file mode 100644 index 0000000..92effdd --- /dev/null +++ b/capabilities2_events/.clang-format @@ -0,0 +1,64 @@ + +BasedOnStyle: Google +AccessModifierOffset: -2 +ConstructorInitializerIndentWidth: 2 +AlignEscapedNewlinesLeft: false +AlignTrailingComments: true +AllowAllParametersOfDeclarationOnNextLine: false +AllowShortIfStatementsOnASingleLine: false +AllowShortLoopsOnASingleLine: false +AllowShortFunctionsOnASingleLine: None +AlwaysBreakTemplateDeclarations: true +AlwaysBreakBeforeMultilineStrings: false +BreakBeforeBinaryOperators: false +BreakBeforeTernaryOperators: false +BreakConstructorInitializersBeforeComma: true +BinPackParameters: true +ColumnLimit: 150 +ConstructorInitializerAllOnOneLineOrOnePerLine: true +DerivePointerBinding: false +PointerBindsToType: true +ExperimentalAutoDetectBinPacking: false +IndentCaseLabels: true +MaxEmptyLinesToKeep: 1 +NamespaceIndentation: None +ObjCSpaceBeforeProtocolList: true +PenaltyBreakBeforeFirstCallParameter: 19 +PenaltyBreakComment: 60 +PenaltyBreakString: 1 +PenaltyBreakFirstLessLess: 1000 +PenaltyExcessCharacter: 1000 +PenaltyReturnTypeOnItsOwnLine: 90 +SpacesBeforeTrailingComments: 2 +Cpp11BracedListStyle: false +Standard: Auto +IndentWidth: 2 +TabWidth: 2 +UseTab: Never +IndentFunctionDeclarationAfterType: false +SpacesInParentheses: false +SpacesInAngles: false +SpaceInEmptyParentheses: false +SpacesInCStyleCastParentheses: false +SpaceAfterControlStatementKeyword: true +SpaceBeforeAssignmentOperators: true +ContinuationIndentWidth: 4 +SortIncludes: false +SpaceAfterCStyleCast: false + +# Configure each individual brace in BraceWrapping +BreakBeforeBraces: Custom + +# Control of individual brace wrapping cases +BraceWrapping: { + AfterClass: 'true' + AfterControlStatement: 'true' + AfterEnum : 'true' + AfterFunction : 'true' + AfterNamespace : 'true' + AfterStruct : 'true' + AfterUnion : 'true' + BeforeCatch : 'true' + BeforeElse : 'true' + IndentBraces : 'false' +} diff --git a/capabilities2_events/CMakeLists.txt b/capabilities2_events/CMakeLists.txt new file mode 100644 index 0000000..bdeb13f --- /dev/null +++ b/capabilities2_events/CMakeLists.txt @@ -0,0 +1,17 @@ +cmake_minimum_required(VERSION 3.8) +project(capabilities2_events) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(capabilities2_msgs REQUIRED) + +install(DIRECTORY include/ + DESTINATION include +) + +ament_package() diff --git a/capabilities2_events/include/capabilities2_events/event_client.hpp b/capabilities2_events/include/capabilities2_events/event_client.hpp new file mode 100644 index 0000000..e5711e4 --- /dev/null +++ b/capabilities2_events/include/capabilities2_events/event_client.hpp @@ -0,0 +1,151 @@ +#pragma once +#include +#include + +/** + * @brief A class to publish events to a given topic + * + */ +class EventClient +{ +public: + using Event = capabilities2_msgs::msg::CapabilityEvent; + + /** + * @brief Construct a new Status Client object + * + * @param node Pointer to the node + * @param node_name node name to be used for status message + * @param topic_name topic name to publish the message + */ + EventClient(rclcpp::Node::SharedPtr node, const std::string& node_name, const std::string& topic_name) + { + node_ = node; + node_name_ = node_name; + event_publisher_ = node_->create_publisher(topic_name, 10); + } + + /** + * @brief publishes status information to the given topic as info + * + * @param text Text to be published + */ + + void info(const std::string& text) + { + auto message = Event(); + + message.header.stamp = node_->now(); + message.origin_node = node_name_; + message.source.capability = ""; + message.source.provider = ""; + message.target.capability = ""; + message.target.provider = ""; + message.thread_id = 0; + message.event = Event::UNDEFINED; + message.error = false; + message.text = text; + message.is_failed_element = false; + message.element = ""; + message.pid = -1; + + event_publisher_->publish(message); + } + + /** + * @brief publishes status information to the given topic as info + * + * @param message Message to be published + */ + void info(const Event& message) + { + message.header.stamp = node_->now(); + message.origin_node = node_name_; + + event_publisher_->publish(message); + } + + /** + * @brief publishes status information to the given topic as error + * + * @param text Text to be published + */ + void error(const std::string& text) + { + auto message = Event(); + + message.header.stamp = node_->now(); + message.origin_node = node_name_; + message.source.capability = ""; + message.source.provider = ""; + message.target.capability = ""; + message.target.provider = ""; + message.thread_id = 0; + message.event = Event::UNDEFINED; + message.error = true; + message.text = text; + message.is_failed_element = false; + message.element = ""; + message.pid = -1; + + event_publisher_->publish(message); + } + + /** + * @brief publishes status information to the given topic as error + * + * @param message Message to be published + */ + void error(const Event& message) + { + message.header.stamp = node_->now(); + message.origin_node = node_name_; + + event_publisher_->publish(message); + } + + /** + * @brief publishes element information to the given topic as error + * + * @param element element information to be published + */ + void error_element(const std::string& element) + { + auto message = Event(); + + message.header.stamp = node_->now(); + message.origin_node = node_name_; + message.source.capability = ""; + message.source.provider = ""; + message.target.capability = ""; + message.target.provider = ""; + message.thread_id = 0; + message.event = Event::UNDEFINED; + message.error = true; + message.text = "Failed element"; + message.is_failed_element = true; + message.element = element; + message.pid = -1; + + event_publisher_->publish(message); + } + +protected: + /** + * @brief Node pointer to access logging interface + * + */ + rclcpp::Node::SharedPtr node_; + + /** + * @brief publisher to publish execution status + * + */ + rclcpp::Publisher::SharedPtr event_publisher_; + + /** + * @brief Node name + * + */ + std::string node_name_; +}; \ No newline at end of file diff --git a/capabilities2_events/package.xml b/capabilities2_events/package.xml new file mode 100644 index 0000000..03d7c01 --- /dev/null +++ b/capabilities2_events/package.xml @@ -0,0 +1,21 @@ + + + + capabilities2_events + 0.0.0 + TODO: Package description + kalana + TODO: License declaration + + ament_cmake + + ament_lint_auto + ament_lint_common + + rclcpp + capabilities2_msgs + + + ament_cmake + + diff --git a/capabilities2_fabric/CMakeLists.txt b/capabilities2_fabric/CMakeLists.txt index 35d8cae..dec67f6 100644 --- a/capabilities2_fabric/CMakeLists.txt +++ b/capabilities2_fabric/CMakeLists.txt @@ -20,6 +20,7 @@ find_package(tinyxml2_vendor REQUIRED) find_package(TinyXML2 REQUIRED) # provided by tinyxml2 upstream, or tinyxml2_vendor find_package(bondcpp REQUIRED) find_package(backward_ros REQUIRED) +find_package(capabilities2_events REQUIRED) include_directories( include @@ -38,6 +39,7 @@ ament_target_dependencies(${PROJECT_NAME} rclcpp_action bondcpp capabilities2_msgs + capabilities2_events TinyXML2 ) @@ -59,6 +61,7 @@ ament_target_dependencies(${PROJECT_NAME}_comp rclcpp_components bondcpp capabilities2_msgs + capabilities2_events TinyXML2 ) @@ -88,6 +91,7 @@ ament_target_dependencies(fabric_client rclcpp rclcpp_action capabilities2_msgs + capabilities2_events TinyXML2 ) @@ -109,6 +113,7 @@ ament_target_dependencies(fabric_client_comp rclcpp_components bondcpp capabilities2_msgs + capabilities2_events TinyXML2 ) diff --git a/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric.hpp b/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric.hpp index b43f799..47e23f6 100644 --- a/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric.hpp +++ b/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric.hpp @@ -10,7 +10,6 @@ #include #include -#include #include @@ -23,6 +22,8 @@ #include #include +#include + /** * @brief Capabilities Fabric * @@ -68,7 +69,7 @@ class CapabilitiesFabric : public rclcpp::Node */ void initialize() { - status_ = std::make_shared(shared_from_this(), "capabilities_fabric", "/status/capabilities_fabric"); + event_ = std::make_shared(shared_from_this(), "capabilities_fabric", "/events/capabilities_fabric"); this->planner_server_ = rclcpp_action::create_server( this, "/capabilities_fabric", std::bind(&CapabilitiesFabric::handle_goal, this, std::placeholders::_1, std::placeholders::_2), @@ -96,8 +97,6 @@ class CapabilitiesFabric : public rclcpp::Node feedback_msg = std::make_shared(); result_msg = std::make_shared(); - - need_reset_ = false; } private: @@ -111,11 +110,11 @@ class CapabilitiesFabric : public rclcpp::Node */ rclcpp_action::GoalResponse handle_goal(const rclcpp_action::GoalUUID& uuid, std::shared_ptr goal) { - status_->info("Received the goal request with the plan"); + event_->info("Received the goal request with the plan"); (void)uuid; - status_->info("Following plan was received :\n\n " + goal->plan); + event_->info("Following plan was received :\n\n " + goal->plan); // try to parse the std::string plan from capabilities_msgs/Plan to the to a XMLDocument file tinyxml2::XMLError xml_status = document.Parse(goal->plan.c_str()); @@ -123,12 +122,11 @@ class CapabilitiesFabric : public rclcpp::Node // check if the file parsing failed if (xml_status != tinyxml2::XMLError::XML_SUCCESS) { - status_->error("Parsing the plan from goal message failed"); + event_->error("Parsing the plan from goal message failed"); return rclcpp_action::GoalResponse::REJECT; } - status_->info("Plan parsed and accepted"); - + event_->info("Plan parsed and accepted"); return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; } @@ -140,7 +138,7 @@ class CapabilitiesFabric : public rclcpp::Node */ rclcpp_action::CancelResponse handle_cancel(const std::shared_ptr goal_handle) { - status_->error("Received the request to cancel the plan"); + event_->info("Received the request to cancel the plan"); (void)goal_handle; for (auto& [bond_id, bond_client] : bond_client_cache_) @@ -159,6 +157,7 @@ class CapabilitiesFabric : public rclcpp::Node void handle_accepted(const std::shared_ptr goal_handle) { goal_handle_ = goal_handle; + event_->info("Goal handle accepted"); execution(); } @@ -168,21 +167,12 @@ class CapabilitiesFabric : public rclcpp::Node */ void execution() { - process_feedback("A new execution started"); + event_->info("A new execution started"); xml_parser::add_closing_event(document); - - std::string modified_plan; - xml_parser::convert_to_string(document, modified_plan); - status_->info("Plan after adding closing event :\n\n " + modified_plan); - - // if (need_reset_) - // { - // free_capability_all(connection_map); - // need_reset_ = false; - // } + event_->info("Plan after adding closing event :\n\n " + modified_plan); interface_list.clear(); providers_list.clear(); @@ -210,7 +200,7 @@ class CapabilitiesFabric : public rclcpp::Node */ void getInterfaces() { - process_feedback("Requesting Interface information"); + event_->info("Requesting Interface information"); auto request_interface = std::make_shared(); @@ -220,14 +210,17 @@ class CapabilitiesFabric : public rclcpp::Node if (!future.valid()) { - process_result("Failed to get Interface information. Server execution cancelled", false, false); + result_msg->success = false; + result_msg->message = "Failed to get Interface information. Server execution cancelled"; + event_->error(result_msg->message); + goal_handle_->abort(result_msg); return; } auto response = future.get(); expected_interfaces_ = response->interfaces.size(); - process_feedback("Received Interfaces. Requsting " + std::to_string(expected_interfaces_) + " semantic interface information"); + event_->info("Received Interfaces. Requsting " + std::to_string(expected_interfaces_) + " semantic interface information"); // Request each interface recursively for Semantic interfaces getSemanticInterfaces(response->interfaces); @@ -243,7 +236,7 @@ class CapabilitiesFabric : public rclcpp::Node { std::string requested_interface = interfaces[completed_interfaces_]; - process_feedback("Requesting semantic interfaces for " + requested_interface, true); + event_->info("Requesting semantic interfaces for " + requested_interface); auto request_semantic = std::make_shared(); request_semantic->interface = requested_interface; @@ -253,7 +246,10 @@ class CapabilitiesFabric : public rclcpp::Node request_semantic, [this, interfaces, requested_interface](GetSemanticInterfacesClient::SharedFuture future) { if (!future.valid()) { - process_result("Failed to get Semantic Interface information. Server execution cancelled", false, false); + result_msg->success = false; + result_msg->message = "Failed to get Semantic Interface information. Server execution cancelled"; + event_->error(result_msg->message); + goal_handle_->abort(result_msg); return; } @@ -268,7 +264,7 @@ class CapabilitiesFabric : public rclcpp::Node interface_list.push_back(semantic_interface); is_semantic_list.push_back(true); - process_feedback(std::to_string(completed_interfaces_) + "/" + std::to_string(expected_interfaces_) + " : Received " + + event_->info(std::to_string(completed_interfaces_) + "/" + std::to_string(expected_interfaces_) + " : Received " + semantic_interface + " for " + requested_interface + ". So added " + semantic_interface); } } @@ -278,7 +274,7 @@ class CapabilitiesFabric : public rclcpp::Node interface_list.push_back(requested_interface); is_semantic_list.push_back(false); - process_feedback(std::to_string(completed_interfaces_) + "/" + std::to_string(expected_interfaces_) + " : Received none for " + + event_->info(std::to_string(completed_interfaces_) + "/" + std::to_string(expected_interfaces_) + " : Received none for " + requested_interface + ". So added " + requested_interface); } @@ -289,11 +285,11 @@ class CapabilitiesFabric : public rclcpp::Node } else { - process_feedback("Received all requested Interface information", true); + event_->info("Received all requested Interface information"); expected_providers_ = interface_list.size(); - process_feedback("Requsting Provider information for " + std::to_string(expected_providers_) + " providers"); + event_->info("Requsting Provider information for " + std::to_string(expected_providers_) + " providers"); // request providers from the interfaces in the interfaces_list getProvider(interface_list, is_semantic_list); @@ -312,7 +308,7 @@ class CapabilitiesFabric : public rclcpp::Node std::string requested_interface = interfaces[completed_providers_]; bool semantic_flag = is_semantic[completed_providers_]; - process_feedback("Requesting provider for " + requested_interface, true); + event_->info("Requesting provider for " + requested_interface); auto request_providers = std::make_shared(); @@ -324,7 +320,10 @@ class CapabilitiesFabric : public rclcpp::Node request_providers, [this, is_semantic, requested_interface, interfaces](GetProvidersClient::SharedFuture future) { if (!future.valid()) { - process_result("Did not retrieve providers for interface: " + requested_interface, false, false); + result_msg->success = false; + result_msg->message = "Failed to retrieve providers for interface: " + requested_interface; + event_->error(result_msg->message); + goal_handle_->abort(result_msg); return; } @@ -336,7 +335,7 @@ class CapabilitiesFabric : public rclcpp::Node // add defualt provider to the list providers_list.push_back(response->default_provider); - process_feedback(std::to_string(completed_providers_) + "/" + std::to_string(expected_providers_) + " : Received " + + event_->info(std::to_string(completed_providers_) + "/" + std::to_string(expected_providers_) + " : Received " + response->default_provider + " for " + requested_interface + ". So added " + response->default_provider); } @@ -347,13 +346,13 @@ class CapabilitiesFabric : public rclcpp::Node { providers_list.push_back(provider); - process_feedback(std::to_string(completed_providers_) + "/" + std::to_string(expected_providers_) + " : Received and added " + + event_->info(std::to_string(completed_providers_) + "/" + std::to_string(expected_providers_) + " : Received and added " + provider + " for " + requested_interface); } } else { - process_feedback(std::to_string(completed_providers_) + "/" + std::to_string(expected_providers_) + " : No providers for " + + event_->info(std::to_string(completed_providers_) + "/" + std::to_string(expected_providers_) + " : No providers for " + requested_interface); } @@ -365,7 +364,7 @@ class CapabilitiesFabric : public rclcpp::Node } else { - process_feedback("All requested interface, semantic interface and provider data recieved", true); + event_->info("All requested interface, semantic interface and provider data recieved"); verify_and_continue(); } @@ -379,12 +378,12 @@ class CapabilitiesFabric : public rclcpp::Node */ void verify_and_continue() { - process_feedback("Verifying the plan"); + event_->info("Verifying the plan"); // verify the plan if (!verify_plan()) { - process_feedback("Plan verification failed"); + event_->info("Plan verification failed"); if (rejected_list.size() > 0) { @@ -399,28 +398,32 @@ class CapabilitiesFabric : public rclcpp::Node } goal_handle_->abort(result); - process_feedback(result->message); + event_->info(result->message); } else { // TODO: improve with error codes - process_result("Plan verification failed. Server Execution Cancelled."); + result_msg->success = false; + result_msg->message = "Plan verification failed. Server Execution Cancelled."; + event_->error(result_msg->message); + goal_handle_->abort(result_msg); + return; } - status_->error("Server Execution Cancelled"); + event_->error("Server Execution Cancelled"); } - process_feedback("Plan verification successful"); + event_->info("Plan verification successful"); // extract the plan from the XMLDocument tinyxml2::XMLElement* plan = xml_parser::get_plan(document); - process_feedback("Plan conversion successful"); + event_->info("Plan conversion successful"); // Extract the connections from the plan xml_parser::extract_connections(plan, connection_map); - process_feedback("Connection extraction successful"); + event_->info("Connection extraction successful"); // estasblish the bond with the server request_bond(); @@ -439,25 +442,31 @@ class CapabilitiesFabric : public rclcpp::Node // verify whether document got 'plan' tags if (!xml_parser::check_plan_tag(document)) { - process_result("Execution plan is not compatible. Please recheck and update"); + result_msg->success = false; + result_msg->message = "Execution plan is not compatible. Please recheck and update"; + event_->error(result_msg->message); + goal_handle_->abort(result_msg); return false; } - process_feedback("'Plan' tag checking successful"); + event_->info("'Plan' tag checking successful"); // extract the components within the 'plan' tags tinyxml2::XMLElement* plan = xml_parser::get_plan(document); - process_feedback("Plan extraction complete"); + event_->info("Plan extraction complete"); // verify whether the plan is valid - if (!xml_parser::check_tags(status_, plan, interface_list, providers_list, control_tag_list, rejected_list)) + if (!xml_parser::check_tags(event_, plan, interface_list, providers_list, control_tag_list, rejected_list)) { - process_result("Execution plan is faulty. Please recheck and update"); + result_msg->success = false; + result_msg->message = "Execution plan is faulty. Please recheck and update"; + event_->error(result_msg->message); + goal_handle_->abort(result_msg); return false; } - process_feedback("Checking tags successful"); + event_->info("Checking tags successful"); return true; } @@ -467,7 +476,7 @@ class CapabilitiesFabric : public rclcpp::Node */ void request_bond() { - process_feedback("Requesting bond id"); + event_->info("Requesting bond id"); // create bond establishing server request auto request_bond = std::make_shared(); @@ -476,13 +485,16 @@ class CapabilitiesFabric : public rclcpp::Node auto result_future = establish_bond_client_->async_send_request(request_bond, [this](EstablishBondClient::SharedFuture future) { if (!future.valid()) { - process_result("Failed to retrieve the bond id. Server execution cancelled"); + result_msg->success = false; + result_msg->message = "Failed to retrieve the bond id. Server execution cancelled"; + event_->error(result_msg->message); + goal_handle_->abort(result_msg); return; } auto response = future.get(); bond_id_ = response->bond_id; - process_feedback("Received the bond id : " + bond_id_); + event_->info("Received the bond id : " + bond_id_); establish_bond(); }); @@ -497,7 +509,7 @@ class CapabilitiesFabric : public rclcpp::Node bond_client_cache_[bond_id_] = std::make_unique(shared_from_this(), bond_id_); bond_client_cache_[bond_id_]->start(); - process_feedback("Bond sucessfully established with bond id : " + bond_id_); + event_->info("Bond sucessfully established with bond id : " + bond_id_); if (bond_client_cache_.size() > 1) { @@ -506,14 +518,14 @@ class CapabilitiesFabric : public rclcpp::Node if (old_bond_id != bond_id_) { bond_client->stop(); - process_feedback("Stopping and removing old bond with id : " + old_bond_id); + event_->info("Stopping and removing old bond with id : " + old_bond_id); } } } expected_capabilities_ = connection_map.size(); - process_feedback("Requsting start of " + std::to_string(expected_capabilities_) + " capabilities"); + event_->info("Requsting start of " + std::to_string(expected_capabilities_) + " capabilities"); use_capability(connection_map); } @@ -534,16 +546,18 @@ class CapabilitiesFabric : public rclcpp::Node request_use->preferred_provider = provider; request_use->bond_id = bond_id_; - process_feedback("Starting capability of Runner " + std::to_string(completed_capabilities_) + " : " + - capabilities[completed_capabilities_].source.runner, - true); + event_->info("Starting capability of Runner " + std::to_string(completed_capabilities_) + " : " + + capabilities[completed_capabilities_].source.runner); // send the request auto result_future = use_capability_client_->async_send_request(request_use, [this, capability, provider](UseCapabilityClient::SharedFuture future) { if (!future.valid()) { - process_result("Failed to Use capability " + capability + " from " + provider + ". Server Execution Cancelled"); + result_msg->success = false; + result_msg->message = "Failed to Use capability " + capability + " from " + provider + ". Server Execution Cancelled"; + event_->error(result_msg->message); + goal_handle_->abort(result_msg); // release all capabilities that were used since not all started successfully free_capability_all(connection_map); @@ -556,20 +570,19 @@ class CapabilitiesFabric : public rclcpp::Node } completed_capabilities_++; - need_reset_ = true; auto response = future.get(); - process_feedback(std::to_string(completed_capabilities_) + "/" + std::to_string(expected_capabilities_) + " : start succeessful"); + event_->info(std::to_string(completed_capabilities_) + "/" + std::to_string(expected_capabilities_) + " : start succeessful"); // Check if all expected calls are completed before calling verify_plan if (completed_capabilities_ == expected_capabilities_) { - process_feedback("All requested capabilities have been started. Configuring the capabilities with events", true); + event_->info("All requested capabilities have been started. Configuring the capabilities with events"); expected_configurations_ = connection_map.size(); - process_feedback("Requsting capability configuration for " + std::to_string(expected_configurations_) + " capabilities", true); + event_->info("Requsting capability configuration for " + std::to_string(expected_configurations_) + " capabilities"); configure_capabilities(connection_map); } @@ -597,19 +610,22 @@ class CapabilitiesFabric : public rclcpp::Node auto result_future = free_capability_client_->async_send_request(request_free, [this, capability](FreeCapabilityClient::SharedFuture future) { if (!future.valid()) { - process_result("Failed to free capability " + capability); + result_msg->success = false; + result_msg->message = "Failed to free capability " + capability; + event_->error(result_msg->message); + goal_handle_->abort(result_msg); return; } auto response = future.get(); - process_feedback("Successfully freed capability " + capability, true); + event_->info("Successfully freed capability " + capability); freed_capabilities_++; // Check if all expected calls are completed before calling verify_plan if (freed_capabilities_ == completed_capabilities_) { - process_feedback("All started capabilities have been freed."); + event_->info("All started capabilities have been freed."); } else { @@ -625,9 +641,8 @@ class CapabilitiesFabric : public rclcpp::Node { auto request_configure = std::make_shared(); - process_feedback("Configuring capability of Runner " + std::to_string(completed_configurations_) + " named " + - capabilities[completed_configurations_].source.runner, - true); + event_->info("Configuring capability of Runner " + std::to_string(completed_configurations_) + " named " + + capabilities[completed_configurations_].source.runner); if (xml_parser::convert_to_string(capabilities[completed_configurations_].source.parameters, request_configure->source.parameters)) { @@ -695,7 +710,10 @@ class CapabilitiesFabric : public rclcpp::Node conf_capability_client_->async_send_request(request_configure, [this, source_capability](ConfigureCapabilityClient::SharedFuture future) { if (!future.valid()) { - process_result("Failed to configure capability :" + source_capability + ". Server execution cancelled"); + result_msg->success = false; + result_msg->message = "Failed to configure capability :" + source_capability + ". Server execution cancelled"; + event_->error(result_msg->message); + goal_handle_->abort(result_msg); return; } @@ -703,13 +721,13 @@ class CapabilitiesFabric : public rclcpp::Node auto response = future.get(); - process_feedback(std::to_string(completed_configurations_) + "/" + std::to_string(expected_configurations_) + + event_->info(std::to_string(completed_configurations_) + "/" + std::to_string(expected_configurations_) + " : Successfully configured capability : " + source_capability); // Check if all expected calls are completed before calling verify_plan if (completed_configurations_ == expected_configurations_) { - process_feedback("All requested capabilities have been configured. Triggering the first capability", true); + event_->info("All requested capabilities have been configured. Triggering the first capability"); trigger_first_node(); } @@ -736,14 +754,20 @@ class CapabilitiesFabric : public rclcpp::Node auto result_future = trig_capability_client_->async_send_request(request_trigger, [this](TriggerCapabilityClient::SharedFuture future) { if (!future.valid()) { - process_result("Failed to trigger capability " + connection_map[0].source.runner); + result_msg->success = false; + result_msg->message = "Failed to trigger capability " + connection_map[0].source.runner; + event_->error(result_msg->message); + goal_handle_->abort(result_msg); return; } auto response = future.get(); - process_feedback("Successfully triggered capability " + connection_map[0].source.runner); + event_->info("Successfully triggered capability " + connection_map[0].source.runner); - process_result("Successfully completed capabilities2 fabric", true); + result_msg->success = true; + result_msg->message = "Successfully completed capabilities2 fabric"; + event_->info(result_msg->message); + goal_handle_->succeed(result_msg); }); } @@ -751,59 +775,22 @@ class CapabilitiesFabric : public rclcpp::Node { while (wait_for_logic) { - status_->error(service_name + " not available"); + event_->error(service_name + " not available"); rclcpp::shutdown(); return; } - - status_->info(service_name + " connected"); - } - - /** - * @brief publishers feedback message and status message - * - * @param text content of the feedback message and status message - * @param newline whether to include a newline before the message - */ - void process_feedback(const std::string& text, bool newline = false) - { - feedback_msg->progress = text; - goal_handle_->publish_feedback(feedback_msg); - - status_->info(text, newline); - } - - /** - * @brief publishers result message and status message - * - * @param text content of the feedback message and status message - * @param success whether the action succeeded - * @param newline whether to include a newline before the message - */ - void process_result(const std::string& text, bool success = false, bool newline = false) - { - result_msg->success = success; - result_msg->message = text; - - if (success) - { - status_->info(text, newline); - goal_handle_->succeed(result_msg); - } - else - { - status_->error(text, newline); - goal_handle_->abort(result_msg); - } + event_->info(service_name + " connected"); } private: /** File Path link */ std::string plan_file_path; + /** Modified plan with closing capabilities */ + std::string modified_plan; + /** flag to select loading from file or accepting via action server */ bool read_file; - bool need_reset_; int expected_interfaces_; int completed_interfaces_; @@ -824,9 +811,6 @@ class CapabilitiesFabric : public rclcpp::Node /** Manages bond between capabilities server and this client */ std::map> bond_client_cache_; - /** Handles status message sending and printing to logging */ - std::shared_ptr status_; - /** XML Document */ tinyxml2::XMLDocument document; @@ -884,6 +868,9 @@ class CapabilitiesFabric : public rclcpp::Node /** trigger an selected capability */ TriggerCapabilityClient::SharedPtr trig_capability_client_; + /** Event client for publishing events */ + std::shared_ptr event_; + /** capabilities2 server and fabric synchronization tools */ // std::mutex mutex_; // std::condition_variable cv_; diff --git a/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric_client.hpp b/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric_client.hpp index 31e65ba..c10dcd4 100644 --- a/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric_client.hpp +++ b/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric_client.hpp @@ -12,16 +12,16 @@ #include #include -#include #include #include - #include #include #include #include +#include + /** * @brief Capabilities Executor File Parser * @@ -55,7 +55,7 @@ class CapabilitiesFabricClient : public rclcpp::Node { fabric_state = Status::IDLE; - status_ = std::make_shared(shared_from_this(), "capabilities_fabric_client", "/status/capabilities_fabric_client"); + event_ = std::make_shared(shared_from_this(), "capabilities_fabric_client", "/events/capabilities_fabric_client"); status_server_ = this->create_service("/capabilities_fabric/get_status", std::bind(&CapabilitiesFabricClient::getStatusCallback, this, @@ -77,12 +77,12 @@ class CapabilitiesFabricClient : public rclcpp::Node if (!this->planner_client_->wait_for_action_server(std::chrono::seconds(5))) { - status_->error("Action server not available after waiting"); + event_->error("Action server not available after waiting"); rclcpp::shutdown(); return; } - status_->info("Sucessfully connected to the capabilities_fabric action server"); + event_->info("Sucessfully connected to the capabilities_fabric action server"); // try to load the file tinyxml2::XMLError xml_status = document.LoadFile(plan_file_path.c_str()); @@ -90,11 +90,11 @@ class CapabilitiesFabricClient : public rclcpp::Node // check if the file loading failed if (xml_status != tinyxml2::XMLError::XML_SUCCESS) { - status_->error("Error loading plan: " + plan_file_path + ", Error: " + document.ErrorName()); + event_->error("Error loading plan: " + plan_file_path + ", Error: " + document.ErrorName()); rclcpp::shutdown(); } - status_->info("Plan loaded from : " + plan_file_path); + event_->info("Plan loaded from : " + plan_file_path); std::string plan; xml_parser::convert_to_string(document, plan); @@ -109,18 +109,18 @@ class CapabilitiesFabricClient : public rclcpp::Node { while (plan_queue.size() > 0) { - status_->info("Fabric client thread starting"); + event_->info("Fabric client thread starting"); std::unique_lock lock(mutex_); completed_ = false; send_goal(); - status_->info("Fabric plan sent. Waiting for acceptance."); + event_->info("Fabric plan sent. Waiting for acceptance."); // Conditional wait cv_.wait(lock, [this] { return completed_; }); - status_->info("Fabric client thread closing"); + event_->info("Fabric client thread closing"); } } @@ -133,7 +133,7 @@ class CapabilitiesFabricClient : public rclcpp::Node goal_msg.plan = plan_queue[0]; plan_queue.pop_front(); - status_->info("Sending goal to the capabilities_fabric action server"); + event_->info("Sending goal to the capabilities_fabric action server"); // send goal options auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); @@ -142,12 +142,12 @@ class CapabilitiesFabricClient : public rclcpp::Node send_goal_options.goal_response_callback = [this](const GoalHandlePlan::SharedPtr& goal_handle) { if (!goal_handle) { - status_->error("Goal was rejected by server"); + event_->error("Goal was rejected by server"); fabric_state = Status::FAILED; } else { - status_->info("Goal accepted by server, waiting for completion"); + event_->info("Goal accepted by server, waiting for completion"); goal_handle_ = goal_handle; fabric_state = Status::RUNNING; } @@ -161,33 +161,33 @@ class CapabilitiesFabricClient : public rclcpp::Node fabric_state = Status::LAUNCHED; break; case rclcpp_action::ResultCode::ABORTED: - status_->error("Goal was aborted"); + event_->error("Goal was aborted"); fabric_state = Status::ABORTED; break; case rclcpp_action::ResultCode::CANCELED: - status_->error("Goal was canceled"); + event_->error("Goal was canceled"); fabric_state = Status::CANCELED; break; default: - status_->error("Unknown result code"); + event_->error("Unknown result code"); fabric_state = Status::FAILED; break; } if (result.result->success) { - status_->info("Plan launched successfully"); + event_->info("Plan launched successfully"); } else { - status_->error("Plan failed to launch"); + event_->error("Plan failed to launch"); if (result.result->failed_elements.size() > 0) { - status_->error("Plan failed due to incompatible XMLElements in the plan"); + event_->error("Plan failed due to incompatible XMLElements in the plan"); for (const auto& failed_element : result.result->failed_elements) - status_->error_element(failed_element); + event_->error_element(failed_element); } } }; @@ -199,7 +199,7 @@ class CapabilitiesFabricClient : public rclcpp::Node { if (fabric_state == Status::RUNNING) { - status_->info("Plan canncelling requested"); + event_->info("Plan canncelling requested"); this->planner_client_->async_cancel_goal(goal_handle_); } @@ -209,7 +209,7 @@ class CapabilitiesFabricClient : public rclcpp::Node void setCompleteCallback(const std::shared_ptr request, std::shared_ptr response) { fabric_state = Status::COMPLETED; - status_->info("Plan completed successfully"); + event_->info("Plan completed successfully"); completed_ = true; cv_.notify_all(); } @@ -252,7 +252,7 @@ class CapabilitiesFabricClient : public rclcpp::Node void setPlanCallback(const std::shared_ptr request, std::shared_ptr response) { - status_->info("Received the request with a plan"); + event_->info("Received the request with a plan"); // try to parse the std::string plan from capabilities_msgs/Plan to the to a XMLDocument file tinyxml2::XMLError xml_status = documentChecking.Parse(request->plan.c_str()); @@ -260,23 +260,23 @@ class CapabilitiesFabricClient : public rclcpp::Node // check if the file parsing failed if (xml_status != tinyxml2::XMLError::XML_SUCCESS) { - status_->info("Parsing the plan from service request message failed"); + event_->info("Parsing the plan from service request message failed"); response->success = false; } - status_->info("Plan parsed and valid"); + event_->info("Plan parsed and valid"); plan_queue.push_back(request->plan); - status_->info("Plan queued and waiting for execution"); + event_->info("Plan queued and waiting for execution"); if ((fabric_state == Status::RUNNING) or (fabric_state == Status::LAUNCHED)) { - status_->info("Prior plan under exeution. Will defer the new plan"); + event_->info("Prior plan under exeution. Will defer the new plan"); } else { - status_->info("Plan parsed and accepted"); + event_->info("Plan parsed and accepted"); goal_send_thread = std::thread(&CapabilitiesFabricClient::manage_goal, this); } @@ -305,9 +305,6 @@ class CapabilitiesFabricClient : public rclcpp::Node /** action client */ rclcpp_action::Client::SharedPtr planner_client_; - /** Handles status message sending and printing to logging */ - std::shared_ptr status_; - /** Goal handle for action client control */ GoalHandlePlan::SharedPtr goal_handle_; @@ -326,18 +323,15 @@ class CapabilitiesFabricClient : public rclcpp::Node /** Status of the fabric */ Status fabric_state; - /** - * @brief mutex for threadpool synchronisation. - */ + /** Event client for publishing events */ + std::shared_ptr event_; + + /** mutex for threadpool synchronisation. */ std::mutex mutex_; - /** - * @brief conditional variable for threadpool synchronisation. - */ + /** conditional variable for threadpool synchronisation */ std::condition_variable cv_; - /** - * @brief flag for threadpool synchronisation. - */ + /** flag for threadpool synchronisation. */ bool completed_; }; diff --git a/capabilities2_fabric/include/capabilities2_fabric/utils/status_client.hpp b/capabilities2_fabric/include/capabilities2_fabric/utils/status_client.hpp deleted file mode 100644 index c634155..0000000 --- a/capabilities2_fabric/include/capabilities2_fabric/utils/status_client.hpp +++ /dev/null @@ -1,110 +0,0 @@ -#pragma once -#include -#include - -class StatusClient -{ -public: - using Status = capabilities2_msgs::msg::Status; - - /** - * @brief Construct a new Status Client object - * - * @param node Pointer to the node - * @param node_name node name to be used for status message - * @param topic_name topic name to publish the message - */ - StatusClient(rclcpp::Node::SharedPtr node, const std::string& node_name,const std::string& topic_name) - { - node_ = node; - node_name_ = node_name; - status_publisher_ = node_->create_publisher(topic_name, 10); - } - - /**error_element - * @brief publishes status information to the given topic and prints to the logging as info - * - * @param text Text to be published - * @param newline whether to add new line to the ROS_INFO print - */ - void info(const std::string &text, bool newline = false) - { - auto status_msg = Status(); - status_msg.header.stamp = node_->now(); - status_msg.origin_node = node_name_; - status_msg.text = text; - status_msg.failure = false; - - if (newline) - RCLCPP_INFO(node_->get_logger(), ""); - - RCLCPP_INFO(node_->get_logger(), status_msg.text.c_str()); - - status_publisher_->publish(status_msg); - } - - /** - * @brief publishes status information to the given topic and prints to the logging as error - * - * @param text Text to be published - * @param newline whether to add new line to the ROS_INFO print - */ - void error(const std::string &text, bool newline = false) - { - auto status_msg = Status(); - status_msg.header.stamp = node_->now(); - status_msg.origin_node = node_name_; - status_msg.text = text; - status_msg.failure = true; - status_msg.is_failed_element = false; - - if (newline) - RCLCPP_ERROR(node_->get_logger(), ""); - - RCLCPP_ERROR(node_->get_logger(), status_msg.text.c_str()); - - status_publisher_->publish(status_msg); - } - - /** - * @brief publishes status information to the given topic and prints to the logging as error - * - * @param text Text to be published - * @param newline whether to add new line to the ROS_INFO print - */ - void error_element(const std::string &element) - { - auto status_msg = Status(); - status_msg.header.stamp = node_->now(); - status_msg.origin_node = node_name_; - status_msg.text = "Failed element"; - status_msg.failure = true; - status_msg.is_failed_element = true; - status_msg.failed_element = element; - - std::string msg = status_msg.text + " : " + status_msg.failed_element; - - RCLCPP_ERROR(node_->get_logger(), msg.c_str()); - - status_publisher_->publish(status_msg); - } - -protected: - /** - * @brief Node pointer to access logging interface - * - */ - rclcpp::Node::SharedPtr node_; - - /** - * @brief publisher to publish execution status - * - */ - rclcpp::Publisher::SharedPtr status_publisher_; - - /** - * @brief Node name - * - */ - std::string node_name_; -}; \ No newline at end of file diff --git a/capabilities2_fabric/include/capabilities2_fabric/utils/xml_parser.hpp b/capabilities2_fabric/include/capabilities2_fabric/utils/xml_parser.hpp index 9defe5a..b8caa18 100644 --- a/capabilities2_fabric/include/capabilities2_fabric/utils/xml_parser.hpp +++ b/capabilities2_fabric/include/capabilities2_fabric/utils/xml_parser.hpp @@ -4,7 +4,7 @@ #include #include #include -#include +#include namespace xml_parser { @@ -123,7 +123,7 @@ void add_closing_event(tinyxml2::XMLDocument& document) * @brief check the plan for invalid/unsupported control and event tags * uses recursive approach to go through the plan * - * @param status StatusClient used for logging and status publishing + * @param event EventClient used for logging and event publishing * @param element XML Element to be evaluated * @param events list containing valid event tags * @param providers list containing providers @@ -132,7 +132,7 @@ void add_closing_event(tinyxml2::XMLDocument& document) * * @return `true` if element valid and supported and `false` otherwise */ -bool check_tags(const std::shared_ptr status, tinyxml2::XMLElement* element, std::vector& events, +bool check_tags(const std::shared_ptr event, tinyxml2::XMLElement* element, std::vector& events, std::vector& providers, std::vector& control, std::vector& rejected) { const char* name; @@ -171,34 +171,34 @@ bool check_tags(const std::shared_ptr status, tinyxml2::XMLElement if (!foundInControl) { std::string msg = "Control tag '" + nametag + "' not available in the valid list"; - status->error(msg); + event->error(msg); rejected.push_back(parameter_string); return false; } if (hasChildren) - returnValue &= xml_parser::check_tags(status, element->FirstChildElement(), events, providers, control, rejected); + returnValue &= xml_parser::check_tags(event, element->FirstChildElement(), events, providers, control, rejected); if (hasSiblings) - returnValue &= xml_parser::check_tags(status, element->NextSiblingElement(), events, providers, control, rejected); + returnValue &= xml_parser::check_tags(event, element->NextSiblingElement(), events, providers, control, rejected); } else if (typetag == "Event") { if (!foundInEvents || !foundInProviders) { std::string msg = "Event tag name '" + nametag + "' or provider '" + providertag + "' not available in the valid list"; - status->error(msg); + event->error(msg); rejected.push_back(parameter_string); return false; } if (hasSiblings) - returnValue &= xml_parser::check_tags(status, element->NextSiblingElement(), events, providers, control, rejected); + returnValue &= xml_parser::check_tags(event, element->NextSiblingElement(), events, providers, control, rejected); } else { std::string msg = "XML element is not valid :" + parameter_string; - status->error(msg); + event->error(msg); rejected.push_back(parameter_string); return false; } diff --git a/capabilities2_fabric/package.xml b/capabilities2_fabric/package.xml index 0e4bc8f..4c47895 100644 --- a/capabilities2_fabric/package.xml +++ b/capabilities2_fabric/package.xml @@ -13,7 +13,8 @@ rclcpp capabilities2_msgs rclcpp_action - backward_ros + backward_ros + capabilities2_events tinyxml2_vendor ament_lint_auto diff --git a/capabilities2_msgs/CMakeLists.txt b/capabilities2_msgs/CMakeLists.txt index 05e02d5..0d19c29 100644 --- a/capabilities2_msgs/CMakeLists.txt +++ b/capabilities2_msgs/CMakeLists.txt @@ -24,7 +24,6 @@ set(msg_files "msg/NaturalCapability.msg" "msg/CapabilityCommand.msg" "msg/CapabilityResponse.msg" - "msg/Status.msg" ) set(srv_files diff --git a/capabilities2_msgs/msg/CapabilityEvent.msg b/capabilities2_msgs/msg/CapabilityEvent.msg index baf45b4..9136623 100644 --- a/capabilities2_msgs/msg/CapabilityEvent.msg +++ b/capabilities2_msgs/msg/CapabilityEvent.msg @@ -1,21 +1,18 @@ std_msgs/Header header +# Node name which published this event +string origin_node + # Capability which this event pertains to Capability source # Capability which this event targets at Capability target -# Capability trigger thread id +# Thread id of the capability which this event pertains to int8 thread_id -# status message -string text - -# status message -string element - -# Event types +# Event types available uint8 IDLE=0 uint8 STARTED=1 uint8 STOPPED=2 @@ -23,17 +20,20 @@ uint8 FAILED=3 uint8 SUCCEEDED=4 uint8 UNDEFINED=5 -# Event type +# Specific event that occurred uint8 event -# Whether the server configuration is complete -bool server_ready - -# Whether a failure occured +# Whether this error relates to a failure bool error -# Whether output data -bool is_element +# status message +string text + +# Whether output data containes xml elements +bool is_failed_element + +# string contianing xml elements +string element # PID of the related process int32 pid diff --git a/capabilities2_msgs/msg/Status.msg b/capabilities2_msgs/msg/Status.msg deleted file mode 100644 index 7d88ea3..0000000 --- a/capabilities2_msgs/msg/Status.msg +++ /dev/null @@ -1,7 +0,0 @@ -std_msgs/Header header -string origin_node -string origin_runner -string text -bool failure -bool is_failed_element -string failed_element \ No newline at end of file From 553f4ea1117637269e4965533bbe25f116cd8727 Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Thu, 1 May 2025 02:55:31 +1000 Subject: [PATCH 104/133] [In progress] revamping event system --- capabilities2_events/CMakeLists.txt | 2 +- .../capabilities2_events/event_client.hpp | 6 - .../capabilities2_events/event_types.hpp | 44 +++++ capabilities2_fabric/CMakeLists.txt | 7 +- .../capabilities_fabric.hpp | 125 +++++++------ .../capabilities_fabric_client.hpp | 30 +++- .../utils/fabric_status.hpp | 15 -- .../capabilities2_fabric/utils/xml_parser.hpp | 2 +- capabilities2_fabric/package.xml | 5 +- capabilities2_runner/CMakeLists.txt | 2 + .../capabilities2_runner/action_runner.hpp | 37 ++-- .../capabilities2_runner/launch_runner.hpp | 5 +- .../capabilities2_runner/runner_base.hpp | 107 +++++------ .../capabilities2_runner/service_runner.hpp | 38 ++-- .../capabilities2_runner/topic_runner.hpp | 38 ++-- .../utils/capability_event_t.hpp | 13 -- capabilities2_runner/package.xml | 1 + capabilities2_runner/src/standard_runners.cpp | 5 +- capabilities2_runner_audio/CMakeLists.txt | 2 + .../listen_runner.hpp | 5 +- .../speak_runner.hpp | 5 +- capabilities2_runner_audio/package.xml | 1 + capabilities2_runner_bt/CMakeLists.txt | 2 + .../bt_factory_runner.hpp | 5 +- capabilities2_runner_bt/package.xml | 1 + .../CMakeLists.txt | 2 + .../capability_get_runner.hpp | 5 +- .../fabric_completion_runner.hpp | 5 +- .../fabric_set_plan_runner.hpp | 5 +- capabilities2_runner_capabilities/package.xml | 1 + capabilities2_runner_nav2/CMakeLists.txt | 2 + .../occupancygrid_runner.hpp | 5 +- .../robotpose_runner.hpp | 47 +++-- .../waypoint_runner.hpp | 5 +- capabilities2_runner_nav2/package.xml | 1 + capabilities2_runner_prompt/CMakeLists.txt | 2 + .../prompt_service_runner.hpp | 5 +- capabilities2_runner_prompt/package.xml | 1 + capabilities2_server/CMakeLists.txt | 3 + .../capabilities2_server/capabilities_api.hpp | 92 ++++------ .../capabilities_server.hpp | 168 ++++++++---------- .../capabilities2_server/runner_cache.hpp | 71 ++------ capabilities2_server/package.xml | 1 + .../src/capabilities_server_node.cpp | 7 +- capabilities2_utils/CMakeLists.txt | 15 ++ .../capabilities2_utils}/bond_client.hpp | 0 .../capabilities2_utils}/connection.hpp | 0 capabilities2_utils/package.xml | 18 ++ 48 files changed, 501 insertions(+), 463 deletions(-) create mode 100644 capabilities2_events/include/capabilities2_events/event_types.hpp delete mode 100644 capabilities2_fabric/include/capabilities2_fabric/utils/fabric_status.hpp delete mode 100644 capabilities2_runner/include/capabilities2_runner/utils/capability_event_t.hpp create mode 100644 capabilities2_utils/CMakeLists.txt rename {capabilities2_fabric/include/capabilities2_fabric/utils => capabilities2_utils/include/capabilities2_utils}/bond_client.hpp (100%) rename {capabilities2_fabric/include/capabilities2_fabric/utils => capabilities2_utils/include/capabilities2_utils}/connection.hpp (100%) create mode 100644 capabilities2_utils/package.xml diff --git a/capabilities2_events/CMakeLists.txt b/capabilities2_events/CMakeLists.txt index bdeb13f..38a1e7e 100644 --- a/capabilities2_events/CMakeLists.txt +++ b/capabilities2_events/CMakeLists.txt @@ -5,7 +5,6 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() - find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(capabilities2_msgs REQUIRED) @@ -14,4 +13,5 @@ install(DIRECTORY include/ DESTINATION include ) +ament_export_include_directories(include) ament_package() diff --git a/capabilities2_events/include/capabilities2_events/event_client.hpp b/capabilities2_events/include/capabilities2_events/event_client.hpp index e5711e4..7803da4 100644 --- a/capabilities2_events/include/capabilities2_events/event_client.hpp +++ b/capabilities2_events/include/capabilities2_events/event_client.hpp @@ -59,9 +59,6 @@ class EventClient */ void info(const Event& message) { - message.header.stamp = node_->now(); - message.origin_node = node_name_; - event_publisher_->publish(message); } @@ -98,9 +95,6 @@ class EventClient */ void error(const Event& message) { - message.header.stamp = node_->now(); - message.origin_node = node_name_; - event_publisher_->publish(message); } diff --git a/capabilities2_events/include/capabilities2_events/event_types.hpp b/capabilities2_events/include/capabilities2_events/event_types.hpp new file mode 100644 index 0000000..e0ddaf0 --- /dev/null +++ b/capabilities2_events/include/capabilities2_events/event_types.hpp @@ -0,0 +1,44 @@ +#pragma once + +namespace capabilities2 +{ +enum event_t +{ + IDLE, + STARTED, + STOPPED, + FAILED, + SUCCEEDED +}; + +/** + * @brief event definition + * + * Contains the informations about the event to be executed. It contains the interface, provider and parameters + */ +struct event +{ + std::string interface; + std::string provider; + std::string parameters; +}; + +/** + * @brief event options + * + * keeps track of events that are related to runner instances at various points of the + * plan + * @param on_started information about the capability to execute on start + * @param on_success information about the capability to execute on success + * @param on_failure information about the capability to execute on failure + * @param on_stopped information about the capability to execute on stop + */ +struct event_opts +{ + event on_started; + event on_success; + event on_failure; + event on_stopped; +}; + +} \ No newline at end of file diff --git a/capabilities2_fabric/CMakeLists.txt b/capabilities2_fabric/CMakeLists.txt index dec67f6..68c0273 100644 --- a/capabilities2_fabric/CMakeLists.txt +++ b/capabilities2_fabric/CMakeLists.txt @@ -15,12 +15,13 @@ find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_action REQUIRED) find_package(rclcpp_components REQUIRED) -find_package(capabilities2_msgs REQUIRED) find_package(tinyxml2_vendor REQUIRED) find_package(TinyXML2 REQUIRED) # provided by tinyxml2 upstream, or tinyxml2_vendor find_package(bondcpp REQUIRED) find_package(backward_ros REQUIRED) +find_package(capabilities2_msgs REQUIRED) find_package(capabilities2_events REQUIRED) +find_package(capabilities2_utils REQUIRED) include_directories( include @@ -40,6 +41,7 @@ ament_target_dependencies(${PROJECT_NAME} bondcpp capabilities2_msgs capabilities2_events + capabilities2_utils TinyXML2 ) @@ -62,6 +64,7 @@ ament_target_dependencies(${PROJECT_NAME}_comp bondcpp capabilities2_msgs capabilities2_events + capabilities2_utils TinyXML2 ) @@ -92,6 +95,7 @@ ament_target_dependencies(fabric_client rclcpp_action capabilities2_msgs capabilities2_events + capabilities2_utils TinyXML2 ) @@ -114,6 +118,7 @@ ament_target_dependencies(fabric_client_comp bondcpp capabilities2_msgs capabilities2_events + capabilities2_utils TinyXML2 ) diff --git a/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric.hpp b/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric.hpp index 47e23f6..4e2c7fe 100644 --- a/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric.hpp +++ b/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric.hpp @@ -9,7 +9,7 @@ #include #include -#include +#include #include @@ -58,7 +58,18 @@ class CapabilitiesFabric : public rclcpp::Node CapabilitiesFabric(const rclcpp::NodeOptions& options = rclcpp::NodeOptions()) : Node("Capabilities2_Fabric", options) { - control_tag_list = xml_parser::get_control_list(); + try + { + // Only call setup if this object is already owned by a shared_ptr + if (shared_from_this()) + { + initialize(); + } + } + catch (const std::bad_weak_ptr&) + { + // Not yet safe — probably standalone without make_shared + } } /** @@ -69,6 +80,8 @@ class CapabilitiesFabric : public rclcpp::Node */ void initialize() { + control_tag_list = xml_parser::get_control_list(); + event_ = std::make_shared(shared_from_this(), "capabilities_fabric", "/events/capabilities_fabric"); this->planner_server_ = rclcpp_action::create_server( @@ -95,7 +108,6 @@ class CapabilitiesFabric : public rclcpp::Node check_service(!trig_capability_client_->wait_for_service(std::chrono::seconds(1)), "/capabilities/trigger_capability"); check_service(!conf_capability_client_->wait_for_service(std::chrono::seconds(1)), "/capabilities/configure_capability"); - feedback_msg = std::make_shared(); result_msg = std::make_shared(); } @@ -264,8 +276,8 @@ class CapabilitiesFabric : public rclcpp::Node interface_list.push_back(semantic_interface); is_semantic_list.push_back(true); - event_->info(std::to_string(completed_interfaces_) + "/" + std::to_string(expected_interfaces_) + " : Received " + - semantic_interface + " for " + requested_interface + ". So added " + semantic_interface); + event_->info(std::to_string(completed_interfaces_) + "/" + std::to_string(expected_interfaces_) + " : Received " + semantic_interface + + " for " + requested_interface + ". So added " + semantic_interface); } } // if no semantic interfaces are availble for a given interface, add the interface instead @@ -275,7 +287,7 @@ class CapabilitiesFabric : public rclcpp::Node is_semantic_list.push_back(false); event_->info(std::to_string(completed_interfaces_) + "/" + std::to_string(expected_interfaces_) + " : Received none for " + - requested_interface + ". So added " + requested_interface); + requested_interface + ". So added " + requested_interface); } if (completed_interfaces_ != expected_interfaces_) @@ -316,59 +328,58 @@ class CapabilitiesFabric : public rclcpp::Node request_providers->interface = requested_interface; request_providers->include_semantic = semantic_flag; - auto result_providers_future = get_providers_client_->async_send_request( - request_providers, [this, is_semantic, requested_interface, interfaces](GetProvidersClient::SharedFuture future) { - if (!future.valid()) - { - result_msg->success = false; - result_msg->message = "Failed to retrieve providers for interface: " + requested_interface; - event_->error(result_msg->message); - goal_handle_->abort(result_msg); - return; - } + auto result_providers_future = get_providers_client_->async_send_request(request_providers, [this, is_semantic, requested_interface, interfaces]( + GetProvidersClient::SharedFuture future) { + if (!future.valid()) + { + result_msg->success = false; + result_msg->message = "Failed to retrieve providers for interface: " + requested_interface; + event_->error(result_msg->message); + goal_handle_->abort(result_msg); + return; + } - completed_providers_++; - auto response = future.get(); + completed_providers_++; + auto response = future.get(); - if (response->default_provider != "") - { - // add defualt provider to the list - providers_list.push_back(response->default_provider); + if (response->default_provider != "") + { + // add defualt provider to the list + providers_list.push_back(response->default_provider); - event_->info(std::to_string(completed_providers_) + "/" + std::to_string(expected_providers_) + " : Received " + - response->default_provider + " for " + requested_interface + ". So added " + response->default_provider); - } + event_->info(std::to_string(completed_providers_) + "/" + std::to_string(expected_providers_) + " : Received " + response->default_provider + + " for " + requested_interface + ". So added " + response->default_provider); + } - // add additional providers to the list if available - if (response->providers.size() > 0) - { - for (const auto& provider : response->providers) - { - providers_list.push_back(provider); + // add additional providers to the list if available + if (response->providers.size() > 0) + { + for (const auto& provider : response->providers) + { + providers_list.push_back(provider); - event_->info(std::to_string(completed_providers_) + "/" + std::to_string(expected_providers_) + " : Received and added " + - provider + " for " + requested_interface); - } - } - else - { - event_->info(std::to_string(completed_providers_) + "/" + std::to_string(expected_providers_) + " : No providers for " + - requested_interface); - } + event_->info(std::to_string(completed_providers_) + "/" + std::to_string(expected_providers_) + " : Received and added " + provider + + " for " + requested_interface); + } + } + else + { + event_->info(std::to_string(completed_providers_) + "/" + std::to_string(expected_providers_) + " : No providers for " + requested_interface); + } - // Check if all expected calls are completed before calling verify_plan - if (completed_providers_ != expected_providers_) - { - // request providers for the next interface in the interfaces_list - getProvider(interfaces, is_semantic); - } - else - { - event_->info("All requested interface, semantic interface and provider data recieved"); + // Check if all expected calls are completed before calling verify_plan + if (completed_providers_ != expected_providers_) + { + // request providers for the next interface in the interfaces_list + getProvider(interfaces, is_semantic); + } + else + { + event_->info("All requested interface, semantic interface and provider data recieved"); - verify_and_continue(); - } - }); + verify_and_continue(); + } + }); } /** @@ -436,7 +447,6 @@ class CapabilitiesFabric : public rclcpp::Node */ bool verify_plan() { - auto feedback = std::make_shared(); auto result = std::make_shared(); // verify whether document got 'plan' tags @@ -547,7 +557,7 @@ class CapabilitiesFabric : public rclcpp::Node request_use->bond_id = bond_id_; event_->info("Starting capability of Runner " + std::to_string(completed_capabilities_) + " : " + - capabilities[completed_capabilities_].source.runner); + capabilities[completed_capabilities_].source.runner); // send the request auto result_future = @@ -642,7 +652,7 @@ class CapabilitiesFabric : public rclcpp::Node auto request_configure = std::make_shared(); event_->info("Configuring capability of Runner " + std::to_string(completed_configurations_) + " named " + - capabilities[completed_configurations_].source.runner); + capabilities[completed_configurations_].source.runner); if (xml_parser::convert_to_string(capabilities[completed_configurations_].source.parameters, request_configure->source.parameters)) { @@ -722,7 +732,7 @@ class CapabilitiesFabric : public rclcpp::Node auto response = future.get(); event_->info(std::to_string(completed_configurations_) + "/" + std::to_string(expected_configurations_) + - " : Successfully configured capability : " + source_capability); + " : Successfully configured capability : " + source_capability); // Check if all expected calls are completed before calling verify_plan if (completed_configurations_ == expected_configurations_) @@ -832,9 +842,6 @@ class CapabilitiesFabric : public rclcpp::Node /** Invalid events list */ std::vector rejected_list; - /** Feedback message for plan action server*/ - std::shared_ptr feedback_msg; - /** Result message for plan action server*/ std::shared_ptr result_msg; diff --git a/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric_client.hpp b/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric_client.hpp index c10dcd4..ebd1fff 100644 --- a/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric_client.hpp +++ b/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric_client.hpp @@ -12,7 +12,6 @@ #include #include -#include #include #include @@ -31,8 +30,18 @@ class CapabilitiesFabricClient : public rclcpp::Node { + enum Status + { + IDLE, + RUNNING, + CANCELED, + ABORTED, + FAILED, + LAUNCHED, + COMPLETED + }; + public: - using Status = capabilities2::fabric_status; using Plan = capabilities2_msgs::action::Plan; using GoalHandlePlan = rclcpp_action::ClientGoalHandle; @@ -43,8 +52,18 @@ class CapabilitiesFabricClient : public rclcpp::Node CapabilitiesFabricClient(const rclcpp::NodeOptions& options = rclcpp::NodeOptions()) : Node("Capabilities2_Fabric_Client", options) { - declare_parameter("plan_file_path", "install/capabilities2_fabric/share/capabilities2_fabric/plans/default.xml"); - plan_file_path = get_parameter("plan_file_path").as_string(); + try + { + // Only call setup if this object is already owned by a shared_ptr + if (shared_from_this()) + { + initialize(); + } + } + catch (const std::bad_weak_ptr&) + { + // Not yet safe — probably standalone without make_shared + } } /** @@ -53,6 +72,9 @@ class CapabilitiesFabricClient : public rclcpp::Node */ void initialize() { + declare_parameter("plan_file_path", "install/capabilities2_fabric/share/capabilities2_fabric/plans/default.xml"); + plan_file_path = get_parameter("plan_file_path").as_string(); + fabric_state = Status::IDLE; event_ = std::make_shared(shared_from_this(), "capabilities_fabric_client", "/events/capabilities_fabric_client"); diff --git a/capabilities2_fabric/include/capabilities2_fabric/utils/fabric_status.hpp b/capabilities2_fabric/include/capabilities2_fabric/utils/fabric_status.hpp deleted file mode 100644 index e862452..0000000 --- a/capabilities2_fabric/include/capabilities2_fabric/utils/fabric_status.hpp +++ /dev/null @@ -1,15 +0,0 @@ -#pragma once - -namespace capabilities2 -{ -enum fabric_status -{ - IDLE, - RUNNING, - CANCELED, - ABORTED, - FAILED, - LAUNCHED, - COMPLETED -}; -} \ No newline at end of file diff --git a/capabilities2_fabric/include/capabilities2_fabric/utils/xml_parser.hpp b/capabilities2_fabric/include/capabilities2_fabric/utils/xml_parser.hpp index b8caa18..4e64e79 100644 --- a/capabilities2_fabric/include/capabilities2_fabric/utils/xml_parser.hpp +++ b/capabilities2_fabric/include/capabilities2_fabric/utils/xml_parser.hpp @@ -3,7 +3,7 @@ #include #include #include -#include +#include #include namespace xml_parser diff --git a/capabilities2_fabric/package.xml b/capabilities2_fabric/package.xml index 4c47895..45bb847 100644 --- a/capabilities2_fabric/package.xml +++ b/capabilities2_fabric/package.xml @@ -11,11 +11,12 @@ ament_cmake rclcpp - capabilities2_msgs rclcpp_action - backward_ros + capabilities2_msgs capabilities2_events + capabilities2_utils tinyxml2_vendor + backward_ros ament_lint_auto ament_lint_common diff --git a/capabilities2_runner/CMakeLists.txt b/capabilities2_runner/CMakeLists.txt index 8774d9c..2c985d7 100644 --- a/capabilities2_runner/CMakeLists.txt +++ b/capabilities2_runner/CMakeLists.txt @@ -19,6 +19,7 @@ find_package(rclcpp REQUIRED) find_package(rclcpp_action REQUIRED) find_package(pluginlib REQUIRED) find_package(capabilities2_msgs REQUIRED) +find_package(capabilities2_events REQUIRED) find_package(tinyxml2_vendor REQUIRED) find_package(TinyXML2 REQUIRED) # provided by tinyxml2 upstream, or tinyxml2_vendor @@ -31,6 +32,7 @@ ament_target_dependencies(${PROJECT_NAME} rclcpp_action pluginlib capabilities2_msgs + capabilities2_events TinyXML2 ) diff --git a/capabilities2_runner/include/capabilities2_runner/action_runner.hpp b/capabilities2_runner/include/capabilities2_runner/action_runner.hpp index 7cb185c..2674f15 100644 --- a/capabilities2_runner/include/capabilities2_runner/action_runner.hpp +++ b/capabilities2_runner/include/capabilities2_runner/action_runner.hpp @@ -39,11 +39,10 @@ class ActionRunner : public RunnerBase * @param run_config runner configuration loaded from the yaml file * @param action_name action name used in the yaml file, used to load specific configuration from the run_config */ - virtual void init_action(rclcpp::Node::SharedPtr node, const runner_opts& run_config, const std::string& action_name, - std::function runner_publish_func) + virtual void init_action(rclcpp::Node::SharedPtr node, const runner_opts& run_config, const std::string& action_name) { // initialize the runner base by storing node pointer and run config - init_base(node, run_config, runner_publish_func); + init_base(node, run_config); // create an action client action_client_ = rclcpp_action::create_client(node_, action_name); @@ -92,10 +91,12 @@ class ActionRunner : public RunnerBase } // Trigger on_stopped event if defined - if (events[execute_id].on_stopped != "") + if (events[execute_id].on_stopped.interface != "") { - info_("on_stopped", -1, events[execute_id].on_stopped, EventType::STOPPED); - triggerFunction_(events[execute_id].on_stopped, update_on_stopped(events[execute_id].on_stopped_param)); + info_("on_stopped", -1, EventType::STOPPED, events[execute_id].on_stopped.interface, + events[execute_id].on_stopped.provider); + triggerFunction_(events[execute_id].on_stopped.interface, + update_on_stopped(events[execute_id].on_stopped.parameters)); } }); @@ -150,10 +151,12 @@ class ActionRunner : public RunnerBase info_("goal accepted. Waiting for result", id); // trigger the events related to on_started state - if (events[execute_id].on_started != "") + if (events[execute_id].on_started.interface != "") { - info_("on_started", id, events[execute_id].on_started, EventType::STARTED); - triggerFunction_(events[execute_id].on_started, update_on_started(events[execute_id].on_started_param)); + info_("on_started", id, EventType::STARTED, events[execute_id].on_started.interface, + events[execute_id].on_started.provider); + triggerFunction_(events[execute_id].on_started.interface, + update_on_started(events[execute_id].on_started.parameters)); } } else @@ -184,10 +187,12 @@ class ActionRunner : public RunnerBase info_("action succeeded.", id); // trigger the events related to on_success state - if (events[execute_id].on_success != "") + if (events[execute_id].on_success.interface != "") { - info_("on_success", id, events[execute_id].on_success, EventType::SUCCEEDED); - triggerFunction_(events[execute_id].on_success, update_on_success(events[execute_id].on_success_param)); + info_("on_success", id, EventType::SUCCEEDED, events[execute_id].on_success.interface, + events[execute_id].on_success.provider); + triggerFunction_(events[execute_id].on_success.interface, + update_on_success(events[execute_id].on_success.parameters)); } } else @@ -195,10 +200,12 @@ class ActionRunner : public RunnerBase error_("action failed", id); // trigger the events related to on_failure state - if (events[execute_id].on_failure != "") + if (events[execute_id].on_failure.interface != "") { - info_("on_failure", id, events[execute_id].on_failure, EventType::FAILED); - triggerFunction_(events[execute_id].on_failure, update_on_failure(events[execute_id].on_failure_param)); + info_("on_failure", id, EventType::FAILED, events[execute_id].on_failure.interface, + events[execute_id].on_failure.provider); + triggerFunction_(events[execute_id].on_failure.interface, + update_on_failure(events[execute_id].on_failure.parameters)); } } diff --git a/capabilities2_runner/include/capabilities2_runner/launch_runner.hpp b/capabilities2_runner/include/capabilities2_runner/launch_runner.hpp index 7673f19..b4c101d 100644 --- a/capabilities2_runner/include/capabilities2_runner/launch_runner.hpp +++ b/capabilities2_runner/include/capabilities2_runner/launch_runner.hpp @@ -29,10 +29,9 @@ class LaunchRunner : public RunnerBase * @param node shared pointer to the capabilities node. Allows to use ros node related functionalities * @param run_config runner configuration loaded from the yaml file */ - virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config, - std::function print) override + virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config) override { - init_base(node, run_config, print); + init_base(node, run_config); package_name = run_config_.runner.substr(0, run_config_.runner.find("/")); launch_name = run_config_.runner.substr(run_config_.runner.find("/") + 1); diff --git a/capabilities2_runner/include/capabilities2_runner/runner_base.hpp b/capabilities2_runner/include/capabilities2_runner/runner_base.hpp index e4f81cb..29cbf29 100644 --- a/capabilities2_runner/include/capabilities2_runner/runner_base.hpp +++ b/capabilities2_runner/include/capabilities2_runner/runner_base.hpp @@ -7,8 +7,9 @@ #include #include #include -#include #include +#include +#include namespace capabilities2_runner { @@ -61,37 +62,11 @@ struct runner_opts std::string pid; }; -/** - * @brief event options - * - * keeps track of events that are related to runner instances at various points of the - * plan - * @param on_started name of the capability to execute on start - * @param on_success name of the capability to execute on success - * @param on_failure name of the capability to execute on failure - * @param on_stopped name of the capability to execute on stop - * @param on_started_param parameters for the capability to execute on start - * @param on_success_param parameters for the capability to execute on success - * @param on_failure_param parameters for the capability to execute on failure - * @param on_stopped_param parameters for the capability to execute on stop - */ -struct event_opts -{ - std::string on_started; - std::string on_success; - std::string on_failure; - std::string on_stopped; - std::string on_started_param; - std::string on_success_param; - std::string on_failure_param; - std::string on_stopped_param; -}; - class RunnerBase { public: using Event = capabilities2_msgs::msg::CapabilityEvent; - using EventType = capabilities2_runner::capability_event; + using EventType = capabilities2::event_t; RunnerBase() : run_config_() { @@ -110,8 +85,7 @@ class RunnerBase * @param run_config runner configuration loaded from the yaml file * @param print_ */ - virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config, - std::function print) = 0; + virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config) = 0; /** * @brief stop the runner @@ -147,17 +121,17 @@ class RunnerBase * @param node shared pointer to the capabilities node. Allows to use ros node related functionalities * @param run_config runner configuration loaded from the yaml file */ - void init_base(rclcpp::Node::SharedPtr node, const runner_opts& run_config, - std::function print) + void init_base(rclcpp::Node::SharedPtr node, const runner_opts& run_config) { // store node pointer and opts node_ = node; run_config_ = run_config; - print_ = print; insert_id = 0; execute_id = -1; thread_id = 0; + + event_ = std::make_shared(node_, "capability_runners", "/events/capability_runners"); } /** @@ -167,7 +141,7 @@ class RunnerBase * * @return number of attached events */ - int attach_events(capabilities2_runner::event_opts& event_option, + int attach_events(capabilities2::event_opts& event_option, std::function triggerFunction) { info_("accepted event options with ID : " + std::to_string(insert_id)); @@ -473,19 +447,23 @@ class RunnerBase } protected: - void info_(const std::string text, int thread_id = -1, const std::string& tcapability = "", - EventType event = EventType::IDLE) + void info_(const std::string text, int thread_id = -1, EventType event = EventType::IDLE, + const std::string& target_capability = "", const std::string& target_provider = "") { auto message = Event(); - message.header.stamp = rclcpp::Clock().now(); + message.header.stamp = node_->now(); + message.origin_node = "capability_runners"; message.source.capability = run_config_.interface; message.source.provider = run_config_.provider; - message.target.capability = tcapability; + message.target.capability = target_capability; + message.target.provider = target_provider; message.thread_id = thread_id; - message.text = text; message.error = false; - message.server_ready = true; + message.text = text; + message.is_failed_element = false; + message.element = ""; + message.pid = -1; switch (event) { @@ -508,43 +486,50 @@ class RunnerBase message.event = Event::UNDEFINED; break; } - - print_(message); + + event_->info(message); } void error_(const std::string text, int thread_id = -1) { auto message = Event(); - message.header.stamp = rclcpp::Clock().now(); + message.header.stamp = node_->now(); + message.origin_node = "capability_runners"; message.source.capability = run_config_.interface; message.source.provider = run_config_.provider; message.target.capability = ""; + message.target.provider = ""; message.thread_id = thread_id; - message.text = text; message.error = true; - message.server_ready = true; - message.event = Event::IDLE; - - print_(message); + message.text = text; + message.is_failed_element = false; + message.element = ""; + message.pid = -1; + message.event = Event::UNDEFINED; + + event_->error(message); } void output_(const std::string text, const std::string element, int thread_id = -1) { auto message = Event(); - message.header.stamp = rclcpp::Clock().now(); + message.header.stamp = node_->now(); + message.origin_node = "capability_runners"; message.source.capability = run_config_.interface; message.source.provider = run_config_.provider; + message.target.capability = ""; + message.target.provider = ""; message.thread_id = thread_id; + message.error = true; message.text = text; + message.is_failed_element = true; message.element = element; - message.is_element = true; - message.error = false; - message.server_ready = true; - message.event = Event::IDLE; - - print_(message); + message.pid = -1; + message.event = Event::UNDEFINED; + + event_->error(message); } /** @@ -560,7 +545,7 @@ class RunnerBase /** * @brief dictionary of events */ - std::map events; + std::map events; /** * @brief Last event tracker id to be inserted @@ -607,11 +592,6 @@ class RunnerBase */ std::function triggerFunction_; - /** - * @brief event function for internal runner event publishing - */ - std::function print_; - /** * @brief XMLElement that is used to convert xml strings to std::string */ @@ -621,6 +601,11 @@ class RunnerBase * @brief XMLElement that is used to convert std::string to xml strings */ tinyxml2::XMLDocument doc; + + /** + * @brief client for publishing events + */ + std::shared_ptr event_; }; } // namespace capabilities2_runner diff --git a/capabilities2_runner/include/capabilities2_runner/service_runner.hpp b/capabilities2_runner/include/capabilities2_runner/service_runner.hpp index 26fe2d4..732255c 100644 --- a/capabilities2_runner/include/capabilities2_runner/service_runner.hpp +++ b/capabilities2_runner/include/capabilities2_runner/service_runner.hpp @@ -32,10 +32,10 @@ class ServiceRunner : public RunnerBase * @param service_name action name used in the yaml file, used to load specific configuration from the run_config */ virtual void init_service(rclcpp::Node::SharedPtr node, const runner_opts& run_config, - const std::string& service_name, std::function print) + const std::string& service_name) { // initialize the runner base by storing node pointer and run config - init_base(node, run_config, print); + init_base(node, run_config); // create an service client service_client_ = node_->create_client(service_name); @@ -82,10 +82,12 @@ class ServiceRunner : public RunnerBase error_("get result call failed"); // trigger the events related to on_failure state - if (events[execute_id].on_failure != "") + if (events[execute_id].on_failure.interface != "") { - info_("on_failure", id, events[execute_id].on_failure, EventType::FAILED); - triggerFunction_(events[execute_id].on_failure, update_on_failure(events[execute_id].on_failure_param)); + info_("on_failure", id, EventType::FAILED, events[execute_id].on_failure.interface, + events[execute_id].on_failure.provider); + triggerFunction_(events[execute_id].on_failure.interface, + update_on_failure(events[execute_id].on_failure.parameters)); } } else @@ -96,10 +98,12 @@ class ServiceRunner : public RunnerBase process_response(response_, id); // trigger the events related to on_success state - if (events[execute_id].on_success != "") + if (events[execute_id].on_success.interface != "") { - info_("on_success", id, events[execute_id].on_success, EventType::SUCCEEDED); - triggerFunction_(events[execute_id].on_success, update_on_success(events[execute_id].on_success_param)); + info_("on_success", id, EventType::SUCCEEDED, events[execute_id].on_success.interface, + events[execute_id].on_success.provider); + triggerFunction_(events[execute_id].on_success.interface, + update_on_success(events[execute_id].on_success.parameters)); } } @@ -108,10 +112,12 @@ class ServiceRunner : public RunnerBase }); // trigger the events related to on_started state - if (events[execute_id].on_started != "") + if (events[execute_id].on_started.interface != "") { - info_("on_started", id, events[execute_id].on_started, EventType::STARTED); - triggerFunction_(events[execute_id].on_started, update_on_started(events[execute_id].on_started_param)); + info_("on_started", id, EventType::STARTED, events[execute_id].on_started.interface, + events[execute_id].on_started.provider); + triggerFunction_(events[execute_id].on_started.interface, + update_on_started(events[execute_id].on_started.parameters)); } // Conditional wait @@ -138,10 +144,12 @@ class ServiceRunner : public RunnerBase throw runner_exception("cannot stop runner action that was not started"); // Trigger on_stopped event if defined - if (events[execute_id].on_stopped != "") + if (events[execute_id].on_stopped.interface != "") { - info_("on_stopped", -1, events[execute_id].on_stopped, EventType::STOPPED); - triggerFunction_(events[execute_id].on_stopped, update_on_stopped(events[execute_id].on_stopped_param)); + info_("on_stopped", -1, EventType::STOPPED, events[execute_id].on_stopped.interface, + events[execute_id].on_stopped.provider); + triggerFunction_(events[execute_id].on_stopped.interface, + update_on_stopped(events[execute_id].on_stopped.parameters)); } info_("stopping runner"); @@ -163,7 +171,7 @@ class ServiceRunner : public RunnerBase /** * @brief Process the reponse and print data as required - * + * * @param response service reponse * @param id thread id */ diff --git a/capabilities2_runner/include/capabilities2_runner/topic_runner.hpp b/capabilities2_runner/include/capabilities2_runner/topic_runner.hpp index 10b49f3..06b3875 100644 --- a/capabilities2_runner/include/capabilities2_runner/topic_runner.hpp +++ b/capabilities2_runner/include/capabilities2_runner/topic_runner.hpp @@ -32,10 +32,10 @@ class TopicRunner : public RunnerBase * @param topic_name topic name used in the yaml file, used to load specific configuration from the run_config */ virtual void init_subscriber(rclcpp::Node::SharedPtr node, const runner_opts& run_config, - const std::string& topic_name, std::function print) + const std::string& topic_name) { // initialize the runner base by storing node pointer and run config - init_base(node, run_config, print); + init_base(node, run_config); // create an service client subscription_ = node_->create_subscription( @@ -58,10 +58,12 @@ class TopicRunner : public RunnerBase throw runner_exception("cannot grab data without parameters"); // trigger the events related to on_started state - if (events[execute_id].on_started != "") + if (events[execute_id].on_started.interface != "") { - info_("on_started", id, events[execute_id].on_started, EventType::STARTED); - triggerFunction_(events[execute_id].on_started, update_on_started(events[execute_id].on_started_param)); + info_("on_started", id, EventType::STARTED, events[execute_id].on_started.interface, + events[execute_id].on_started.provider); + triggerFunction_(events[execute_id].on_started.interface, + update_on_started(events[execute_id].on_started.parameters)); } std::unique_lock lock(mutex_); @@ -76,10 +78,12 @@ class TopicRunner : public RunnerBase if (latest_message_) { // trigger the events related to on_success state - if (events[execute_id].on_success != "") + if (events[execute_id].on_success.interface != "") { - info_("on_success", id, events[execute_id].on_success, EventType::SUCCEEDED); - triggerFunction_(events[execute_id].on_success, update_on_success(events[execute_id].on_success_param)); + info_("on_success", id, EventType::SUCCEEDED, events[execute_id].on_success.interface, + events[execute_id].on_success.provider); + triggerFunction_(events[execute_id].on_success.interface, + update_on_success(events[execute_id].on_success.parameters)); } } else @@ -87,10 +91,12 @@ class TopicRunner : public RunnerBase error_("Message receving failed."); // trigger the events related to on_failure state - if (events[execute_id].on_failure != "") + if (events[execute_id].on_failure.interface != "") { - info_("on_failure", id, events[execute_id].on_failure, EventType::FAILED); - triggerFunction_(events[execute_id].on_failure, update_on_failure(events[execute_id].on_failure_param)); + info_("on_failure", id, EventType::FAILED, events[execute_id].on_failure.interface, + events[execute_id].on_failure.provider); + triggerFunction_(events[execute_id].on_failure.interface, + update_on_failure(events[execute_id].on_failure.parameters)); } } @@ -116,12 +122,14 @@ class TopicRunner : public RunnerBase throw runner_exception("cannot stop runner subscriber that was not started"); // Trigger on_stopped event if defined - if (events[execute_id].on_stopped != "") + if (events[execute_id].on_stopped.interface != "") { - info_("on_stopped", -1, events[execute_id].on_stopped, EventType::STOPPED); - triggerFunction_(events[execute_id].on_stopped, update_on_stopped(events[execute_id].on_stopped_param)); + info_("on_stopped", -1, EventType::STOPPED, events[execute_id].on_stopped.interface, + events[execute_id].on_stopped.provider); + triggerFunction_(events[execute_id].on_stopped.interface, + update_on_stopped(events[execute_id].on_stopped.parameters)); } - + info_("stopping runner"); } diff --git a/capabilities2_runner/include/capabilities2_runner/utils/capability_event_t.hpp b/capabilities2_runner/include/capabilities2_runner/utils/capability_event_t.hpp deleted file mode 100644 index afe4dde..0000000 --- a/capabilities2_runner/include/capabilities2_runner/utils/capability_event_t.hpp +++ /dev/null @@ -1,13 +0,0 @@ -#pragma once - -namespace capabilities2_runner -{ -enum capability_event -{ - IDLE, - STARTED, - STOPPED, - FAILED, - SUCCEEDED -}; -} \ No newline at end of file diff --git a/capabilities2_runner/package.xml b/capabilities2_runner/package.xml index 117ddcf..69f7e1c 100644 --- a/capabilities2_runner/package.xml +++ b/capabilities2_runner/package.xml @@ -20,6 +20,7 @@ pluginlib std_msgs capabilities2_msgs + capabilities2_events tinyxml2_vendor diff --git a/capabilities2_runner/src/standard_runners.cpp b/capabilities2_runner/src/standard_runners.cpp index e62d9e0..58ca354 100644 --- a/capabilities2_runner/src/standard_runners.cpp +++ b/capabilities2_runner/src/standard_runners.cpp @@ -7,11 +7,10 @@ namespace capabilities2_runner class DummyRunner : public RunnerBase { public: - void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config, - std::function runner_publish_func) override + void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config) override { // init the base runner - init_base(node, run_config, runner_publish_func); + init_base(node, run_config); // do nothing RCLCPP_INFO(node_->get_logger(), "Dummy runner started"); diff --git a/capabilities2_runner_audio/CMakeLists.txt b/capabilities2_runner_audio/CMakeLists.txt index 1e3797a..7f28773 100644 --- a/capabilities2_runner_audio/CMakeLists.txt +++ b/capabilities2_runner_audio/CMakeLists.txt @@ -18,6 +18,7 @@ find_package(pluginlib REQUIRED) find_package(hri_audio_msgs REQUIRED) find_package(capabilities2_msgs REQUIRED) find_package(capabilities2_runner REQUIRED) +find_package(capabilities2_events REQUIRED) find_package(tinyxml2_vendor REQUIRED) find_package(TinyXML2 REQUIRED) # provided by tinyxml2 upstream, or tinyxml2_vendor @@ -36,6 +37,7 @@ ament_target_dependencies(${PROJECT_NAME} hri_audio_msgs capabilities2_msgs capabilities2_runner + capabilities2_events TinyXML2 ) 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 1985914..ad86511 100644 --- a/capabilities2_runner_audio/include/capabilities2_runner_audio/listen_runner.hpp +++ b/capabilities2_runner_audio/include/capabilities2_runner_audio/listen_runner.hpp @@ -31,10 +31,9 @@ class ListenerRunner : public ActionRunner * @param node shared pointer to the capabilities node. Allows to use ros node related functionalities * @param run_config runner configuration loaded from the yaml file */ - virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config, - std::function runner_publish_func) override + virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config) override { - init_action(node, run_config, "speech_to_text", runner_publish_func); + init_action(node, run_config, "speech_to_text"); } protected: diff --git a/capabilities2_runner_audio/include/capabilities2_runner_audio/speak_runner.hpp b/capabilities2_runner_audio/include/capabilities2_runner_audio/speak_runner.hpp index a776f63..bd2d623 100644 --- a/capabilities2_runner_audio/include/capabilities2_runner_audio/speak_runner.hpp +++ b/capabilities2_runner_audio/include/capabilities2_runner_audio/speak_runner.hpp @@ -31,10 +31,9 @@ class SpeakerRunner : public ActionRunner * @param node shared pointer to the capabilities node. Allows to use ros node related functionalities * @param run_config runner configuration loaded from the yaml file */ - virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config, - std::function runner_publish_func) override + virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config) override { - init_action(node, run_config, "text_to_speech", runner_publish_func); + init_action(node, run_config, "text_to_speech"); } protected: diff --git a/capabilities2_runner_audio/package.xml b/capabilities2_runner_audio/package.xml index 970a446..128348b 100644 --- a/capabilities2_runner_audio/package.xml +++ b/capabilities2_runner_audio/package.xml @@ -17,6 +17,7 @@ hri_audio_msgs capabilities2_msgs capabilities2_runner + capabilities2_events tinyxml2_vendor diff --git a/capabilities2_runner_bt/CMakeLists.txt b/capabilities2_runner_bt/CMakeLists.txt index 1d6386e..81c5fcc 100644 --- a/capabilities2_runner_bt/CMakeLists.txt +++ b/capabilities2_runner_bt/CMakeLists.txt @@ -16,6 +16,7 @@ find_package(rclcpp_action REQUIRED) find_package(pluginlib REQUIRED) find_package(capabilities2_msgs REQUIRED) find_package(capabilities2_runner REQUIRED) +find_package(capabilities2_events REQUIRED) find_package(behaviortree_cpp REQUIRED) find_package(tinyxml2_vendor REQUIRED) find_package(TinyXML2 REQUIRED) # provided by tinyxml2 upstream, or tinyxml2_vendor @@ -34,6 +35,7 @@ ament_target_dependencies(${PROJECT_NAME} pluginlib capabilities2_msgs capabilities2_runner + capabilities2_events behaviortree_cpp TinyXML2 ) diff --git a/capabilities2_runner_bt/include/capabilities2_runner_bt/bt_factory_runner.hpp b/capabilities2_runner_bt/include/capabilities2_runner_bt/bt_factory_runner.hpp index c760350..846f1a8 100644 --- a/capabilities2_runner_bt/include/capabilities2_runner_bt/bt_factory_runner.hpp +++ b/capabilities2_runner_bt/include/capabilities2_runner_bt/bt_factory_runner.hpp @@ -21,11 +21,10 @@ class BTRunnerBase : public RunnerBase, public BT::BehaviorTreeFactory { } - virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config, - std::function print) + virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config) { // init the runner base - init_base(node, run_config, print); + init_base(node, run_config); // register (bt)actions from ROS plugins try diff --git a/capabilities2_runner_bt/package.xml b/capabilities2_runner_bt/package.xml index 1be5306..3dded60 100644 --- a/capabilities2_runner_bt/package.xml +++ b/capabilities2_runner_bt/package.xml @@ -21,6 +21,7 @@ std_msgs capabilities2_msgs capabilities2_runner + capabilities2_events tinyxml2_vendor behaviortree_cpp diff --git a/capabilities2_runner_capabilities/CMakeLists.txt b/capabilities2_runner_capabilities/CMakeLists.txt index 9e99551..c90344e 100644 --- a/capabilities2_runner_capabilities/CMakeLists.txt +++ b/capabilities2_runner_capabilities/CMakeLists.txt @@ -16,6 +16,7 @@ find_package(rclcpp_action REQUIRED) find_package(pluginlib REQUIRED) find_package(capabilities2_msgs REQUIRED) find_package(capabilities2_runner REQUIRED) +find_package(capabilities2_events REQUIRED) find_package(tinyxml2_vendor REQUIRED) find_package(TinyXML2 REQUIRED) # provided by tinyxml2 upstream, or tinyxml2_vendor @@ -33,6 +34,7 @@ ament_target_dependencies(${PROJECT_NAME} pluginlib capabilities2_msgs capabilities2_runner + capabilities2_events TinyXML2 ) diff --git a/capabilities2_runner_capabilities/include/capabilities2_runner_capabilities/capability_get_runner.hpp b/capabilities2_runner_capabilities/include/capabilities2_runner_capabilities/capability_get_runner.hpp index 89b03f1..e6d70ad 100644 --- a/capabilities2_runner_capabilities/include/capabilities2_runner_capabilities/capability_get_runner.hpp +++ b/capabilities2_runner_capabilities/include/capabilities2_runner_capabilities/capability_get_runner.hpp @@ -31,10 +31,9 @@ class CapabilityGetRunner : public ServiceRunner print) override + virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config) override { - init_service(node, run_config, "/capabilities/get_capability_specs", print); + init_service(node, run_config, "/capabilities/get_capability_specs"); } protected: diff --git a/capabilities2_runner_capabilities/include/capabilities2_runner_capabilities/fabric_completion_runner.hpp b/capabilities2_runner_capabilities/include/capabilities2_runner_capabilities/fabric_completion_runner.hpp index d849dbf..c07d7ca 100644 --- a/capabilities2_runner_capabilities/include/capabilities2_runner_capabilities/fabric_completion_runner.hpp +++ b/capabilities2_runner_capabilities/include/capabilities2_runner_capabilities/fabric_completion_runner.hpp @@ -27,10 +27,9 @@ class FabricCompletionRunner : public ServiceRunner print) override + virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config) override { - init_service(node, run_config, "/capabilities_fabric/set_completion", print); + init_service(node, run_config, "/capabilities_fabric/set_completion"); } protected: diff --git a/capabilities2_runner_capabilities/include/capabilities2_runner_capabilities/fabric_set_plan_runner.hpp b/capabilities2_runner_capabilities/include/capabilities2_runner_capabilities/fabric_set_plan_runner.hpp index 3d83b04..7c85deb 100644 --- a/capabilities2_runner_capabilities/include/capabilities2_runner_capabilities/fabric_set_plan_runner.hpp +++ b/capabilities2_runner_capabilities/include/capabilities2_runner_capabilities/fabric_set_plan_runner.hpp @@ -31,10 +31,9 @@ class FabricSetPlanRunner : public ServiceRunner print) override + virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config) override { - init_service(node, run_config, "/capabilities_fabric/set_plan", print); + init_service(node, run_config, "/capabilities_fabric/set_plan"); } protected: diff --git a/capabilities2_runner_capabilities/package.xml b/capabilities2_runner_capabilities/package.xml index 7546f41..c383eda 100644 --- a/capabilities2_runner_capabilities/package.xml +++ b/capabilities2_runner_capabilities/package.xml @@ -18,6 +18,7 @@ std_msgs capabilities2_msgs capabilities2_runner + capabilities2_events tinyxml2_vendor diff --git a/capabilities2_runner_nav2/CMakeLists.txt b/capabilities2_runner_nav2/CMakeLists.txt index 6194b9f..29e59b1 100644 --- a/capabilities2_runner_nav2/CMakeLists.txt +++ b/capabilities2_runner_nav2/CMakeLists.txt @@ -20,6 +20,7 @@ find_package(tf2_ros REQUIRED) find_package(geometry_msgs REQUIRED) find_package(capabilities2_msgs REQUIRED) find_package(capabilities2_runner REQUIRED) +find_package(capabilities2_events REQUIRED) find_package(tinyxml2_vendor REQUIRED) find_package(TinyXML2 REQUIRED) # provided by tinyxml2 upstream, or tinyxml2_vendor @@ -41,6 +42,7 @@ ament_target_dependencies(${PROJECT_NAME} geometry_msgs capabilities2_msgs capabilities2_runner + capabilities2_events TinyXML2 ) diff --git a/capabilities2_runner_nav2/include/capabilities2_runner_nav2/occupancygrid_runner.hpp b/capabilities2_runner_nav2/include/capabilities2_runner_nav2/occupancygrid_runner.hpp index ede210c..7856b7d 100644 --- a/capabilities2_runner_nav2/include/capabilities2_runner_nav2/occupancygrid_runner.hpp +++ b/capabilities2_runner_nav2/include/capabilities2_runner_nav2/occupancygrid_runner.hpp @@ -29,10 +29,9 @@ class OccupancyGridRunner : public TopicRunner * @param node shared pointer to the capabilities node. Allows to use ros node related functionalities * @param run_config runner configuration loaded from the yaml file */ - virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config, - std::function print) override + virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config) override { - init_subscriber(node, run_config, "/map", print); + init_subscriber(node, run_config, "/map"); } protected: diff --git a/capabilities2_runner_nav2/include/capabilities2_runner_nav2/robotpose_runner.hpp b/capabilities2_runner_nav2/include/capabilities2_runner_nav2/robotpose_runner.hpp index 298949d..f42efb5 100644 --- a/capabilities2_runner_nav2/include/capabilities2_runner_nav2/robotpose_runner.hpp +++ b/capabilities2_runner_nav2/include/capabilities2_runner_nav2/robotpose_runner.hpp @@ -33,11 +33,10 @@ class RobotPoseRunner : public RunnerBase * @param node shared pointer to the capabilities node. Allows to use ros node related functionalities * @param run_config runner configuration loaded from the yaml file */ - virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config, - std::function runner_publish_func) override + virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config) override { // initialize the runner base by storing node pointer and run config - init_base(node, run_config, runner_publish_func); + init_base(node, run_config); tf_buffer_ = std::make_unique(node_->get_clock()); tf_listener_ = std::make_shared(*tf_buffer_); @@ -63,10 +62,12 @@ class RobotPoseRunner : public RunnerBase throw runner_exception("cannot grab data without parameters"); // trigger the events related to on_started state - if (events[execute_id].on_started != "") + if (events[execute_id].on_started.interface != "") { - info_("on_started", id, events[execute_id].on_started, EventType::STARTED); - triggerFunction_(events[execute_id].on_started, update_on_started(events[execute_id].on_started_param)); + info_("on_started", id, EventType::STARTED, events[execute_id].on_started.interface, + events[execute_id].on_started.provider); + triggerFunction_(events[execute_id].on_started.interface, + update_on_started(events[execute_id].on_started.parameters)); } info_("Waiting for Transformation.", id); @@ -87,12 +88,14 @@ class RobotPoseRunner : public RunnerBase transform_ = tf_buffer_->lookupTransform(mapFrame, robotFrame, tf2::TimePointZero); // trigger the events related to on_success state - if (events[execute_id].on_success != "") + if (events[execute_id].on_success.interface != "") { - info_("on_success", id, events[execute_id].on_success, EventType::SUCCEEDED); - triggerFunction_(events[execute_id].on_success, update_on_success(events[execute_id].on_success_param)); + info_("on_success", id, EventType::SUCCEEDED, events[execute_id].on_success.interface, + events[execute_id].on_success.provider); + triggerFunction_(events[execute_id].on_success.interface, + update_on_success(events[execute_id].on_success.parameters)); } - + info_("Transformation received. Thread closing.", id); return; } @@ -107,10 +110,12 @@ class RobotPoseRunner : public RunnerBase transform_ = tf_buffer_->lookupTransform(odomFrame, robotFrame, tf2::TimePointZero); // trigger the events related to on_success state - if (events[execute_id].on_success != "") + if (events[execute_id].on_success.interface != "") { - info_("on_success", id, events[execute_id].on_success, EventType::SUCCEEDED); - triggerFunction_(events[execute_id].on_success, update_on_success(events[execute_id].on_success_param)); + info_("on_success", id, EventType::SUCCEEDED, events[execute_id].on_success.interface, + events[execute_id].on_success.provider); + triggerFunction_(events[execute_id].on_success.interface, + update_on_success(events[execute_id].on_success.parameters)); } info_("Transformation received. Thread closing.", id); @@ -120,10 +125,12 @@ class RobotPoseRunner : public RunnerBase info_("Could not transform from odom to robot: " + std::string(ex.what()), id); // trigger the events related to on_failure state - if (events[execute_id].on_failure != "") + if (events[execute_id].on_failure.interface != "") { - info_("on_failure", id, events[execute_id].on_failure, EventType::FAILED); - triggerFunction_(events[execute_id].on_failure, update_on_failure(events[execute_id].on_failure_param)); + info_("on_failure", id, EventType::FAILED, events[execute_id].on_failure.interface, + events[execute_id].on_failure.provider); + triggerFunction_(events[execute_id].on_failure.interface, + update_on_failure(events[execute_id].on_failure.parameters)); } info_("Transformation not received. Thread closing.", id); @@ -149,10 +156,12 @@ class RobotPoseRunner : public RunnerBase throw runner_exception("cannot stop runner subscriber that was not started"); // Trigger on_stopped event if defined - if (events[execute_id].on_stopped != "") + if (events[execute_id].on_stopped.interface != "") { - info_("on_stopped", -1, events[execute_id].on_stopped, EventType::STOPPED); - triggerFunction_(events[execute_id].on_stopped, update_on_stopped(events[execute_id].on_stopped_param)); + info_("on_stopped", -1, EventType::STOPPED, events[execute_id].on_stopped.interface, + events[execute_id].on_stopped.provider); + triggerFunction_(events[execute_id].on_stopped.interface, + update_on_stopped(events[execute_id].on_stopped.parameters)); } } 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 1a93591..bf2ad5c 100644 --- a/capabilities2_runner_nav2/include/capabilities2_runner_nav2/waypoint_runner.hpp +++ b/capabilities2_runner_nav2/include/capabilities2_runner_nav2/waypoint_runner.hpp @@ -36,10 +36,9 @@ class WayPointRunner : public ActionRunner * @param node shared pointer to the capabilities node. Allows to use ros node related functionalities * @param run_config runner configuration loaded from the yaml file */ - virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config, - std::function runner_publish_func) override + virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config) override { - init_action(node, run_config, "navigate_to_pose", runner_publish_func); + init_action(node, run_config, "navigate_to_pose"); } protected: diff --git a/capabilities2_runner_nav2/package.xml b/capabilities2_runner_nav2/package.xml index 305326a..cb8ff67 100644 --- a/capabilities2_runner_nav2/package.xml +++ b/capabilities2_runner_nav2/package.xml @@ -16,6 +16,7 @@ geometry_msgs capabilities2_msgs capabilities2_runner + capabilities2_events tinyxml2_vendor ament_lint_auto diff --git a/capabilities2_runner_prompt/CMakeLists.txt b/capabilities2_runner_prompt/CMakeLists.txt index ca72255..6fb5ef2 100644 --- a/capabilities2_runner_prompt/CMakeLists.txt +++ b/capabilities2_runner_prompt/CMakeLists.txt @@ -17,6 +17,7 @@ find_package(pluginlib REQUIRED) find_package(prompt_msgs REQUIRED) find_package(capabilities2_msgs REQUIRED) find_package(capabilities2_runner REQUIRED) +find_package(capabilities2_events REQUIRED) find_package(tinyxml2_vendor REQUIRED) find_package(TinyXML2 REQUIRED) # provided by tinyxml2 upstream, or tinyxml2_vendor @@ -35,6 +36,7 @@ ament_target_dependencies(${PROJECT_NAME} prompt_msgs capabilities2_msgs capabilities2_runner + capabilities2_events TinyXML2 ) diff --git a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_service_runner.hpp b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_service_runner.hpp index 5ec9959..a60ba9f 100644 --- a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_service_runner.hpp +++ b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_service_runner.hpp @@ -28,10 +28,9 @@ class PromptServiceRunner : public ServiceRunner * @param node shared pointer to the capabilities node. Allows to use ros node related functionalities * @param run_config runner configuration loaded from the yaml file */ - virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config, - std::function print) override + virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config) override { - init_service(node, run_config, "/prompt_bridge/prompt", print); + init_service(node, run_config, "/prompt_bridge/prompt"); } protected: diff --git a/capabilities2_runner_prompt/package.xml b/capabilities2_runner_prompt/package.xml index 0f56363..f1d245b 100644 --- a/capabilities2_runner_prompt/package.xml +++ b/capabilities2_runner_prompt/package.xml @@ -15,6 +15,7 @@ prompt_msgs capabilities2_msgs capabilities2_runner + capabilities2_events tinyxml2_vendor ament_lint_auto diff --git a/capabilities2_server/CMakeLists.txt b/capabilities2_server/CMakeLists.txt index 1e7e4c0..05e7d1a 100644 --- a/capabilities2_server/CMakeLists.txt +++ b/capabilities2_server/CMakeLists.txt @@ -19,6 +19,7 @@ find_package(pluginlib REQUIRED) find_package(rclcpp_components REQUIRED) find_package(capabilities2_msgs REQUIRED) find_package(capabilities2_runner REQUIRED) +find_package(capabilities2_events REQUIRED) find_package(tinyxml2_vendor REQUIRED) find_package(TinyXML2 REQUIRED) # provided by tinyxml2 upstream, or tinyxml2_vendor find_package(backward_ros REQUIRED) @@ -60,6 +61,7 @@ ament_target_dependencies(${PROJECT_NAME} rclcpp_components capabilities2_msgs capabilities2_runner + capabilities2_events TinyXML2 SQLite3 yaml-cpp @@ -102,6 +104,7 @@ ament_target_dependencies(${PROJECT_NAME}_node rclcpp_components capabilities2_msgs capabilities2_runner + capabilities2_events TinyXML2 SQLite3 yaml-cpp diff --git a/capabilities2_server/include/capabilities2_server/capabilities_api.hpp b/capabilities2_server/include/capabilities2_server/capabilities_api.hpp index 291c211..9717d1a 100644 --- a/capabilities2_server/include/capabilities2_server/capabilities_api.hpp +++ b/capabilities2_server/include/capabilities2_server/capabilities_api.hpp @@ -16,7 +16,8 @@ #include #include #include -#include +#include +#include #include #include @@ -46,22 +47,19 @@ class CapabilitiesAPI * @brief connect with the database file * * @param db_file file path of the database file - * @param node_logging_interface_ptr pointer to the ROS node logging interface + * @param event_client pointer to the event publishing interface */ - void connect(const std::string& db_file, rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logging, - std::function print, - std::function runner_print) + void connect(const std::string& db_file, std::shared_ptr event_client) { - print_ = print; - logging_ = logging; + event_ = event_client; - runner_cache_.connect(print, runner_print, logging); + runner_cache_.connect(event_client); // connect db cap_db_ = std::make_unique(db_file); // log - print_("Capabilities API connected to db: " + db_file, false, false); + event_->info("Capabilities API connected to db: " + db_file); } /** @@ -85,7 +83,7 @@ class CapabilitiesAPI // go through the running model and start the necessary dependencies for (const auto& run : running.dependencies) { - print_("found dependency: " + run.interface, true, false); + event_->info("found dependency: " + run.interface); // make an internal 'use' bond for the capability dependency bind_dependency(run.interface); @@ -106,13 +104,13 @@ class CapabilitiesAPI { runner_cache_.add_runner(node, capability, run_config); - print_("started capability: " + capability + " with provider: " + provider, true, false); + event_->info("started capability: " + capability + " with provider: " + provider); return value and true; } catch (const capabilities2_runner::runner_exception& e) { - RCLCPP_WARN(logging_->get_logger(), "could not start runner: %s", e.what()); + event_->error("could not start runner: " + std::string(e.what())); return false; } } @@ -135,7 +133,7 @@ class CapabilitiesAPI } catch (const capabilities2_runner::runner_exception& e) { - RCLCPP_WARN(logging_->get_logger(), "could not trigger runner: %s", e.what()); + event_->error("could not trigger runner: " + std::string(e.what())); } } @@ -150,7 +148,7 @@ class CapabilitiesAPI // this can happen if dependencies fail to resolve in the first place if (!runner_cache_.running(capability)) { - print_("could not get provider for: " + capability, true, true); + event_->error("could not get provider for: " + capability); return; } @@ -164,7 +162,7 @@ class CapabilitiesAPI // FIXME: this unrolls the dependency tree from the bottom up but should probably be top down for (const auto& run : running.dependencies) { - print_("freeing dependency: " + run.interface + "of : " + capability, true, false); + event_->info("freeing dependency: " + run.interface + "of : " + capability); // remove the internal 'use' bond for the capability dependency unbind_dependency(run.interface); @@ -180,16 +178,16 @@ class CapabilitiesAPI // this will implicitly stop the runner try { - runner_cache_.remove_runner(capability); + runner_cache_.remove_runner(capability); } catch (const capabilities2_runner::runner_exception& e) { - RCLCPP_WARN(logging_->get_logger(), "could not stop runner: %s", e.what()); + event_->error("could not stop runner: " + std::string(e.what())); return; } // log - print_("stopped capability: " + capability, true, false); + event_->info("stopped capability: " + capability); } /** @@ -208,7 +206,7 @@ class CapabilitiesAPI if (!bond_cache_.exists(capability)) { // stop the capability - print_("stopping freed capability: " + capability, true, false); + event_->info("stopping freed capability: " + capability); stop_capability(capability); } @@ -239,35 +237,22 @@ class CapabilitiesAPI * @brief Set triggers for `on_success`, `on_failure`, `on_start`, `on_stop` events for a given capability * * @param capability capability from where the events originate - * @param on_started_capability capability triggered by on_start event - * @param on_started_parameters parameters related to capability triggered by on_start event - * @param on_stopped_capability capability triggered by on_stop event - * @param on_stopped_parameters parameters related to capability triggered by on_stop event - * @param on_success_capability capability triggered by on_success event - * @param on_success_parameters parameters related to capability triggered by on_success event - * @param on_failure_capability capability triggered by on_failure event - * @param on_failure_parameters parameters related to capability triggered by on_failure event + * @param event_options event options for the capability */ - void set_triggers(const std::string& capability, const std::string& on_started_capability, - const std::string& on_started_parameters, const std::string& on_failure_capability, - const std::string& on_failure_parameters, const std::string& on_success_capability, - const std::string& on_success_parameters, const std::string& on_stopped_capability, - const std::string& on_stopped_parameters) + void set_triggers(const std::string& capability, capabilities2::event_opts& event_options) { try { // log - print_("Setting triggers for capability: " + capability, true, false); + event_->info("Setting triggers for capability: " + capability); - runner_cache_.set_runner_triggers(capability, on_started_capability, on_started_parameters, on_failure_capability, - on_failure_parameters, on_success_capability, on_success_parameters, - on_stopped_capability, on_stopped_parameters); + runner_cache_.set_runner_triggers(capability, event_options); - print_("Successfully set triggers for capability: " + capability, true, false); + event_->info("Successfully set triggers for capability: " + capability); } catch (const capabilities2_runner::runner_exception& e) { - RCLCPP_WARN(logging_->get_logger(), "Sould not set triggers: %s", e.what()); + event_->error("could not set triggers for the runner: " + std::string(e.what())); } } @@ -285,7 +270,7 @@ class CapabilitiesAPI // exists guard if (cap_db_->exists(header.name)) { - RCLCPP_WARN(logging_->get_logger(), "interface already exists"); + event_->info(header.name + " interface already exists"); return; } @@ -297,7 +282,7 @@ class CapabilitiesAPI } catch (const std::exception& e) { - print_("failed to convert spec to model: " + std::string(e.what()), true, true); + event_->error("failed to convert spec to model: " + std::string(e.what())); return; } @@ -305,7 +290,7 @@ class CapabilitiesAPI model.header.name = spec.package + "/" + model.header.name; cap_db_->insert_interface(model); - print_("interface added to db: " + model.header.name, true, false); + event_->info("interface added to db: " + model.header.name); return; } @@ -314,7 +299,7 @@ class CapabilitiesAPI { if (cap_db_->exists(header.name)) { - RCLCPP_WARN(logging_->get_logger(), "semantic interface already exists"); + event_->info(header.name + " semantic interface already exists"); return; } @@ -326,7 +311,7 @@ class CapabilitiesAPI } catch (const std::exception& e) { - print_("failed to convert spec to model: " + std::string(e.what()), true, true); + event_->error("failed to convert spec to model: " + std::string(e.what())); return; } @@ -334,7 +319,7 @@ class CapabilitiesAPI cap_db_->insert_semantic_interface(model); // log - print_("semantic interface added to db: " + model.header.name, true, false); + event_->info("semantic interface added to db: " + model.header.name); return; } @@ -343,7 +328,7 @@ class CapabilitiesAPI { if (cap_db_->exists(header.name)) { - RCLCPP_WARN(logging_->get_logger(), "provider already exists"); + event_->info(header.name + " provider already exists"); return; } @@ -355,7 +340,7 @@ class CapabilitiesAPI } catch (const std::exception& e) { - print_("failed to convert spec to model: " + std::string(e.what()), true, true); + event_->error("failed to convert spec to model: " + std::string(e.what())); return; } @@ -363,12 +348,12 @@ class CapabilitiesAPI cap_db_->insert_provider(model); // log - print_("provider added to db: " + model.header.name, true, false); + event_->info("provider added to db: " + model.header.name); return; } // couldn't parse unknown capability type - print_("unknown capability type: " + spec.type, true, true); + event_->error("unknown capability type: " + spec.type); } // query api @@ -627,13 +612,13 @@ class CapabilitiesAPI void on_bond_established(const std::string& bond_id) { // log bond established event - RCLCPP_INFO(logging_->get_logger(), "bond established with id: %s", bond_id.c_str()); + event_->error("bond established with id: " + bond_id); } void on_bond_broken(const std::string& bond_id) { // log warning - RCLCPP_WARN(logging_->get_logger(), "bond broken for id: %s", bond_id.c_str()); + event_->error("bond broken for id: " + bond_id); // get capabilities requested by the bond std::vector capabilities = bond_cache_.get_capabilities(bond_id); @@ -707,8 +692,8 @@ class CapabilitiesAPI // db std::unique_ptr cap_db_; - // for internal logging - rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logging_; + // for events publishing + std::shared_ptr event_; // caches BondCache bond_cache_; @@ -716,9 +701,6 @@ class CapabilitiesAPI // internal bindings std::vector internal_bond_ids_; - - // event function for external event publishing - std::function print_; }; } // namespace capabilities2_server diff --git a/capabilities2_server/include/capabilities2_server/capabilities_server.hpp b/capabilities2_server/include/capabilities2_server/capabilities_server.hpp index 6746c65..3570fd1 100644 --- a/capabilities2_server/include/capabilities2_server/capabilities_server.hpp +++ b/capabilities2_server/include/capabilities2_server/capabilities_server.hpp @@ -34,6 +34,9 @@ #include #include +#include +#include + namespace capabilities2_server { @@ -59,9 +62,29 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI public: CapabilitiesServer(const rclcpp::NodeOptions& options = rclcpp::NodeOptions()) : Node("capabilities2", options), CapabilitiesAPI() + { + try + { + // Only call setup if this object is already owned by a shared_ptr + if (shared_from_this()) + { + initialize(); + } + } + catch (const std::bad_weak_ptr&) + { + // Not yet safe — probably standalone without make_shared + } + } + + /** + * @brief Initializes the capabilities server node. + * + */ + void initialize() { // pubs - event_pub_ = create_publisher("~/events", 10); + event_ = std::make_shared(shared_from_this(), "capabilities_server", "/events/capabilities_server"); // params interface // loop rate @@ -87,11 +110,11 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI if (rebuild) { // remove db file if it exists - event_publish("Removing the old capabilities database", false); + event_->info("Removing the old capabilities database"); if (std::remove(db_file.c_str()) != 0) { - event_publish("Error deleting database file " + db_file, false, true); + event_->error("Error deleting database file " + db_file); } } @@ -99,20 +122,17 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI if (!std::filesystem::exists(db_path)) { // create db file path - event_publish("Creating capabilities database", false); + event_->info("Creating capabilities database"); std::filesystem::create_directories(db_path.parent_path()); } // init capabilities api - event_publish("Connecting API with Database", false); + event_->info("Connecting API with Database"); - connect(db_file, get_node_logging_interface(), - std::bind(&capabilities2_server::CapabilitiesServer::event_publish, this, std::placeholders::_1, - std::placeholders::_2, std::placeholders::_3), - std::bind(&capabilities2_server::CapabilitiesServer::event_publish_runner, this, std::placeholders::_1)); + connect(db_file, event_); - event_publish("Loading capabilities", false); + event_->info("Loading capabilities"); // load capabilities from package paths for (const auto& package_path : package_paths) @@ -120,7 +140,7 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI load_capabilities(package_path); } - event_publish("Starting server interfaces", false); + event_->info("Starting server interfaces"); // services // establish bond @@ -193,7 +213,7 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI std::placeholders::_1, std::placeholders::_2)); // publish ready event - event_publish("capabilities server start up complete"); + event_->info("capabilities server start up complete"); } // service callbacks @@ -249,13 +269,13 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI // guard empty values if (req->capability.empty()) { - event_publish("free_capability: capability is empty", true, true); + event_->error("free_capability: capability is empty"); return; } if (req->bond_id.empty()) { - event_publish("free_capability: bond_id is empty", true, true); + event_->error("free_capability: bond_id is empty"); return; } @@ -272,19 +292,19 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI // guard empty values if (req->capability.empty()) { - event_publish("use_capability: capability is empty", true, true); + event_->error("use_capability: capability is empty"); return; } if (req->preferred_provider.empty()) { - event_publish("use_capability: preferred_provider is empty", true, true); + event_->error("use_capability: preferred_provider is empty"); return; } if (req->bond_id.empty()) { - event_publish("use_capability: bond_id is empty", true, true); + event_->error("use_capability: bond_id is empty"); return; } @@ -298,11 +318,26 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI void configure_capability_cb(const std::shared_ptr req, std::shared_ptr res) { + capabilities2::event_opts event_options; + + event_options.on_started.interface = req->target_on_start.capability; + event_options.on_started.provider = req->target_on_start.provider; + event_options.on_started.parameters = req->target_on_start.parameters; + + event_options.on_failure.interface = req->target_on_failure.capability; + event_options.on_failure.provider = req->target_on_failure.provider; + event_options.on_failure.parameters = req->target_on_failure.parameters; + + event_options.on_success.interface = req->target_on_success.capability; + event_options.on_success.provider = req->target_on_success.provider; + event_options.on_success.parameters = req->target_on_success.parameters; + + event_options.on_stopped.interface = req->target_on_stop.capability; + event_options.on_stopped.provider = req->target_on_stop.provider; + event_options.on_stopped.parameters = req->target_on_stop.parameters; + // setup triggers between parameters - set_triggers(req->source.capability, req->target_on_start.capability, req->target_on_start.parameters, - req->target_on_failure.capability, req->target_on_failure.parameters, - req->target_on_success.capability, req->target_on_success.parameters, req->target_on_stop.capability, - req->target_on_stop.parameters); + set_triggers(req->source.capability, event_options); // response is empty } @@ -314,7 +349,7 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI // guard empty values if (req->capability_spec.package.empty() || req->capability_spec.content.empty()) { - event_publish("register_capability: package or content is empty", true, true); + event_->error("register_capability: package or content is empty"); return; } @@ -364,7 +399,7 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI // if the spec is not empty set response if (capability_spec.content.empty()) { - event_publish("capability spec not found for resource: " + req->capability_spec, true, true); + event_->error("capability spec not found for resource: " + req->capability_spec); // BUG: throw error causes service to crash, this is a ROS2 bug // std::runtime_error("capability spec not found for resource: " + req->capability_spec); @@ -407,12 +442,12 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI private: void load_capabilities(const std::string& package_path) { - RCLCPP_DEBUG(get_logger(), "Loading capabilities from package path: %s", package_path.c_str()); + event_->info("Loading capabilities from package path: " + package_path); // check if path exists if (!std::filesystem::exists(package_path)) { - event_publish("package path does not exist: " + package_path, true, true); + event_->error("package path does not exist: " + package_path); return; } @@ -439,7 +474,7 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI // load capabilities from packages in /opt/ros/*/share for (const auto& package : packages_root) { - RCLCPP_DEBUG(get_logger(), "loading capabilities from package: %s", package.c_str()); + event_->info("Loading capabilities from package: " + package); // package.xml exports std::string package_xml = package_path + "/" + package + "/package.xml"; @@ -447,7 +482,7 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI // check if package.xml exists if (!std::filesystem::exists(package_xml)) { - RCLCPP_DEBUG(get_logger(), "package.xml does not exist: %s", package_xml.c_str()); + event_->error("package.xml does not exist: " + package_xml); continue; } @@ -460,7 +495,7 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI } catch (const std::runtime_error& e) { - event_publish("failed to parse package.xml file: " + std::string(e.what()), true, true); + event_->error("failed to parse package.xml file: " + std::string(e.what())); continue; } @@ -469,7 +504,7 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI if (exports == nullptr) { - RCLCPP_DEBUG(get_logger(), "No exports found in package.xml file: %s", package_xml.c_str()); + event_->error("No exports found in package.xml file: " + package_xml); continue; } @@ -500,13 +535,13 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI load_spec_content(package_path + "/" + package + "/" + spec_path, capability_spec); // add capability to db - event_publish("adding capability: " + package + "/" + spec_path); + event_->info("adding capability: " + package + "/" + spec_path); add_capability(capability_spec); } catch (const std::runtime_error& e) { - event_publish("failed to load spec file: " + std::string(e.what()), true, true); + event_->error("failed to load spec file: " + std::string(e.what())); } } }; @@ -522,7 +557,7 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI // load capabilities from packages in workspace install folder for (const auto& package : packages_install) { - RCLCPP_DEBUG(get_logger(), "loading capabilities from package: %s", package.c_str()); + event_->info("Loading capabilities from package: " + package); // package.xml exports std::string package_xml = package_path + "/" + package + "/share/" + package + "/package.xml"; @@ -530,7 +565,7 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI // check if package.xml exists if (!std::filesystem::exists(package_xml)) { - RCLCPP_DEBUG(get_logger(), "package.xml does not exist: %s", package_xml.c_str()); + event_->error("package.xml does not exist: " + package_xml); continue; } @@ -543,7 +578,7 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI } catch (const std::runtime_error& e) { - event_publish("failed to parse package.xml file: " + std::string(e.what()), true, true); + event_->error("failed to parse package.xml file: " + std::string(e.what())); continue; } @@ -552,7 +587,7 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI if (exports == nullptr) { - RCLCPP_DEBUG(get_logger(), "No exports found in package.xml file: %s", package_xml.c_str()); + event_->error("No exports found in package.xml file: " + package_xml); continue; } @@ -583,13 +618,13 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI load_spec_content(package_path + "/" + package + "/share/" + package + "/" + spec_path, capability_spec); // add capability to db - event_publish("adding capability: " + package + "/" + spec_path); + event_->info("adding capability: " + package + "/" + spec_path); add_capability(capability_spec); } catch (const std::runtime_error& e) { - event_publish("failed to load spec file: " + std::string(e.what()), true, true); + event_->error("failed to load spec file: " + std::string(e.what())); } } }; @@ -655,68 +690,13 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI spec_file_file.close(); } - void event_publish(const std::string& text, bool is_server_ready = true, bool is_error = false) - { - auto message = capabilities2_msgs::msg::CapabilityEvent(); - - message.header.stamp = rclcpp::Clock().now(); - message.source.capability = ""; - message.source.provider = ""; - message.target.capability = ""; - message.target.provider = ""; - message.thread_id = 0; - message.text = text; - message.error = is_error; - message.pid = -1; - message.server_ready = is_server_ready; - - event_pub_->publish(message); - - if (is_error) - RCLCPP_ERROR(get_logger(), text.c_str()); - else - RCLCPP_INFO(get_logger(), text.c_str()); - } - - void event_publish_runner(capabilities2_msgs::msg::CapabilityEvent& message) - { - message.pid = get_pid(message.source.capability); - message.server_ready = true; - - event_pub_->publish(message); - - std::string text; - if (message.thread_id >= 0 and message.target.capability == "") - { - text = "[" + message.source.capability + "/" + std::to_string(message.thread_id) + "] " + message.text; - } - else if (message.thread_id < 0 and message.target.capability == "") - { - text = "[" + message.source.capability + "] " + message.text; - } - else if (message.thread_id >= 0 and message.target.capability != "") - { - text = "[" + message.source.capability + "/" + std::to_string(message.thread_id) + "] triggering " + - message.target.capability + " " + message.text; - } - else if (message.thread_id < 0 and message.target.capability != "") - { - text = "[" + message.source.capability + "] triggering " + message.target.capability + " " + message.text; - } - - if (message.error) - RCLCPP_ERROR(get_logger(), text.c_str()); - else - RCLCPP_INFO(get_logger(), text.c_str()); - } - private: // loop hz double loop_hz_; // publishers - // event publisher - rclcpp::Publisher::SharedPtr event_pub_; + /** Event client for publishing events */ + std::shared_ptr event_; // services // establish bond diff --git a/capabilities2_server/include/capabilities2_server/runner_cache.hpp b/capabilities2_server/include/capabilities2_server/runner_cache.hpp index 416612c..1d48bf7 100644 --- a/capabilities2_server/include/capabilities2_server/runner_cache.hpp +++ b/capabilities2_server/include/capabilities2_server/runner_cache.hpp @@ -9,6 +9,7 @@ #include #include #include +#include namespace capabilities2_server { @@ -29,27 +30,16 @@ class RunnerCache public: RunnerCache() : runner_loader_("capabilities2_runner", "capabilities2_runner::RunnerBase") { - // on_started = nullptr; - // on_stopped = nullptr; - // on_failure = nullptr; - // on_success = nullptr; } /** - * @brief connect with ROS node logging interface + * @brief connect with event interface * - * @param print function to publish INFO messages to the ROS environment - * @param runner_print function to be passed into runners for publishing INFO messages to the ROS environment - * @param logging pointer to the ROS node logging interface for WARN and ERROR messages + * @param event_client pointer to the event client */ - void connect(std::function print, - std::function runner_print, - rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logging) + void connect(std::shared_ptr event_client) { - // set logger - logging_ = logging; - print_ = print; - runner_print_ = runner_print; + event_ = event_client; } /** @@ -91,7 +81,7 @@ class RunnerCache } // start the runner - runner_cache_[capability]->start(node, run_config.to_runner_opts(), runner_print_); + runner_cache_[capability]->start(node, run_config.to_runner_opts()); } /** @@ -111,7 +101,7 @@ class RunnerCache } else { - print_("Runner not found for capability: " + capability, true, true); + event_->error("Runner not found for capability: " + capability); throw capabilities2_runner::runner_exception("capability runner not found: " + capability); } } @@ -121,40 +111,21 @@ class RunnerCache * * * @param capability capability from where the events originate - * @param on_started_capability capability triggered by on_start event - * @param on_started_parameters parameters related to capability triggered by on_start event - * @param on_stopped_capability capability triggered by on_stop event - * @param on_stopped_parameters parameters related to capability triggered by on_stop event - * @param on_success_capability capability triggered by on_success event - * @param on_success_parameters parameters related to capability triggered by on_success event - * @param on_failure_capability capability triggered by on_failure event - * @param on_failure_parameters parameters related to capability triggered by on_failure event + * @param on_started on_start event with capability and parameters + * @param on_failure on_failure event with capability and parameters + * @param on_success on_success event with capability and parameters + * @param on_stopped on_stop event with capability and parameters */ - void set_runner_triggers(const std::string& capability, const std::string& on_started_capability, - const std::string& on_started_parameters, const std::string& on_failure_capability, - const std::string& on_failure_parameters, const std::string& on_success_capability, - const std::string& on_success_parameters, const std::string& on_stopped_capability, - const std::string& on_stopped_parameters) + void set_runner_triggers(const std::string& capability, capabilities2::event_opts& event_options) { - capabilities2_runner::event_opts event_options; - - event_options.on_started = on_started_capability; - event_options.on_failure = on_failure_capability; - event_options.on_success = on_success_capability; - event_options.on_stopped = on_stopped_capability; - - event_options.on_started_param = on_started_parameters; - event_options.on_failure_param = on_failure_parameters; - event_options.on_success_param = on_success_parameters; - event_options.on_stopped_param = on_stopped_parameters; - int event_count = runner_cache_[capability]->attach_events( event_options, std::bind(&capabilities2_server::RunnerCache::trigger_runner, this, std::placeholders::_1, std::placeholders::_2)); - print_("Configured triggers for capability " + capability + ": \n\tStarted: " + on_started_capability + - " \n\tFailure: " + on_failure_capability + " \n\tSuccess: " + on_success_capability + - "\n\tStopped: " + on_stopped_capability + "\n", true, false); + event_->info( + "Configured triggers for capability " + capability + ": \n\tStarted: " + event_options.on_started.interface + + " \n\tFailure: " + event_options.on_failure.interface + " \n\tSuccess: " + event_options.on_success.interface + + "\n\tStopped: " + event_options.on_stopped.interface); } /** @@ -267,14 +238,8 @@ class RunnerCache // runner plugin loader pluginlib::ClassLoader runner_loader_; - // logger - rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logging_; - - // event function for external event publishing - std::function print_; - - // event function for internal runner event publishing - std::function runner_print_; + // for events publishing + std::shared_ptr event_; // event string std::string event; diff --git a/capabilities2_server/package.xml b/capabilities2_server/package.xml index 8d70790..bbe4230 100644 --- a/capabilities2_server/package.xml +++ b/capabilities2_server/package.xml @@ -22,6 +22,7 @@ rclcpp_components capabilities2_msgs capabilities2_runner + capabilities2_events tinyxml2_vendor diff --git a/capabilities2_server/src/capabilities_server_node.cpp b/capabilities2_server/src/capabilities_server_node.cpp index 48e5bb6..2817989 100644 --- a/capabilities2_server/src/capabilities_server_node.cpp +++ b/capabilities2_server/src/capabilities_server_node.cpp @@ -7,11 +7,16 @@ int main(int argc, char** argv) // Create the node object and executor object auto node = std::make_shared(); + + // Initialize the node + node->initialize(); + + // Create a MultiThreadedExecutor auto exec = std::make_shared(); // Add the node to the executor exec->add_node(node); - + // Spin the executor exec->spin(); diff --git a/capabilities2_utils/CMakeLists.txt b/capabilities2_utils/CMakeLists.txt new file mode 100644 index 0000000..db84b39 --- /dev/null +++ b/capabilities2_utils/CMakeLists.txt @@ -0,0 +1,15 @@ +cmake_minimum_required(VERSION 3.8) +project(capabilities2_utils) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) + +install(DIRECTORY include/ + DESTINATION include +) + +ament_export_include_directories(include) +ament_package() diff --git a/capabilities2_fabric/include/capabilities2_fabric/utils/bond_client.hpp b/capabilities2_utils/include/capabilities2_utils/bond_client.hpp similarity index 100% rename from capabilities2_fabric/include/capabilities2_fabric/utils/bond_client.hpp rename to capabilities2_utils/include/capabilities2_utils/bond_client.hpp diff --git a/capabilities2_fabric/include/capabilities2_fabric/utils/connection.hpp b/capabilities2_utils/include/capabilities2_utils/connection.hpp similarity index 100% rename from capabilities2_fabric/include/capabilities2_fabric/utils/connection.hpp rename to capabilities2_utils/include/capabilities2_utils/connection.hpp diff --git a/capabilities2_utils/package.xml b/capabilities2_utils/package.xml new file mode 100644 index 0000000..de33cec --- /dev/null +++ b/capabilities2_utils/package.xml @@ -0,0 +1,18 @@ + + + + capabilities2_utils + 0.0.0 + TODO: Package description + kalana + TODO: License declaration + + ament_cmake + + ament_lint_auto + ament_lint_common + + + ament_cmake + + From bd8cc733e3144c08ab861afa06077fd9ae77163a Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Tue, 6 May 2025 19:22:55 +1000 Subject: [PATCH 105/133] added event listener for debugging purposes --- capabilities2_events/CMakeLists.txt | 38 +++++++++++- .../capabilities2_events/event_client.hpp | 12 ++-- .../capabilities2_events/event_listener.hpp | 61 +++++++++++++++++++ .../launch/listener.launch.py | 28 +++++++++ capabilities2_events/package.xml | 1 + .../src/event_listener_node.cpp | 17 ++++++ capabilities2_fabric/CMakeLists.txt | 4 +- .../docs/prompt_capability_runner_ex1.md | 7 +++ .../docs/prompt_occupancy_runner_ex1.md | 7 +++ .../docs/prompt_plan_runner_ex1.md | 7 +++ .../docs/prompt_pose_runner_ex1.md | 7 +++ .../docs/waypoint_runner_ex1.md | 7 +++ .../docs/waypoint_runner_ex2.md | 7 +++ .../capabilities_fabric.hpp | 2 +- .../capabilities_fabric_client.hpp | 2 +- ... capabilities_fabric_client_component.cpp} | 0 .../src/capabilities_fabric_client_node.cpp | 5 ++ ....cpp => capabilities_fabric_component.cpp} | 0 capabilities2_runner/CMakeLists.txt | 8 +-- .../capabilities2_runner/runner_base.hpp | 2 +- capabilities2_server/CMakeLists.txt | 26 ++++---- .../capabilities_server.hpp | 10 +-- ....cpp => capabilities_server_component.cpp} | 0 23 files changed, 224 insertions(+), 34 deletions(-) create mode 100644 capabilities2_events/include/capabilities2_events/event_listener.hpp create mode 100644 capabilities2_events/launch/listener.launch.py create mode 100644 capabilities2_events/src/event_listener_node.cpp rename capabilities2_fabric/src/{capabilities_fabric_client_comp.cpp => capabilities_fabric_client_component.cpp} (100%) rename capabilities2_fabric/src/{capabilities_fabric_comp.cpp => capabilities_fabric_component.cpp} (100%) rename capabilities2_server/src/{capabilities_server_comp.cpp => capabilities_server_component.cpp} (100%) diff --git a/capabilities2_events/CMakeLists.txt b/capabilities2_events/CMakeLists.txt index 38a1e7e..52b569a 100644 --- a/capabilities2_events/CMakeLists.txt +++ b/capabilities2_events/CMakeLists.txt @@ -1,6 +1,11 @@ -cmake_minimum_required(VERSION 3.8) +cmake_minimum_required(VERSION 3.10) project(capabilities2_events) +# Default to C++17 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() + if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() @@ -9,9 +14,40 @@ find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(capabilities2_msgs REQUIRED) +include_directories( + include +) + +############################################################################ +# node implementation that compiles as a executable +############################################################################ + +add_executable(${PROJECT_NAME}_node + src/event_listener_node.cpp +) + +# target_link_libraries(${PROJECT_NAME}_node) + +ament_target_dependencies(${PROJECT_NAME}_node + rclcpp + capabilities2_msgs +) + +install(TARGETS ${PROJECT_NAME}_node + DESTINATION lib/${PROJECT_NAME} +) + +############################################################################ +# miscellaneous +############################################################################ + install(DIRECTORY include/ DESTINATION include ) +install(DIRECTORY launch + DESTINATION share/${PROJECT_NAME} +) + ament_export_include_directories(include) ament_package() diff --git a/capabilities2_events/include/capabilities2_events/event_client.hpp b/capabilities2_events/include/capabilities2_events/event_client.hpp index 7803da4..9ff3f79 100644 --- a/capabilities2_events/include/capabilities2_events/event_client.hpp +++ b/capabilities2_events/include/capabilities2_events/event_client.hpp @@ -31,7 +31,7 @@ class EventClient * @param text Text to be published */ - void info(const std::string& text) + void info(const std::string& text, int thread_id = -1) { auto message = Event(); @@ -41,7 +41,7 @@ class EventClient message.source.provider = ""; message.target.capability = ""; message.target.provider = ""; - message.thread_id = 0; + message.thread_id = thread_id; message.event = Event::UNDEFINED; message.error = false; message.text = text; @@ -67,7 +67,7 @@ class EventClient * * @param text Text to be published */ - void error(const std::string& text) + void error(const std::string& text, int thread_id = -1) { auto message = Event(); @@ -77,7 +77,7 @@ class EventClient message.source.provider = ""; message.target.capability = ""; message.target.provider = ""; - message.thread_id = 0; + message.thread_id = thread_id; message.event = Event::UNDEFINED; message.error = true; message.text = text; @@ -103,7 +103,7 @@ class EventClient * * @param element element information to be published */ - void error_element(const std::string& element) + void error_element(const std::string& element, int thread_id = -1) { auto message = Event(); @@ -113,7 +113,7 @@ class EventClient message.source.provider = ""; message.target.capability = ""; message.target.provider = ""; - message.thread_id = 0; + message.thread_id = thread_id; message.event = Event::UNDEFINED; message.error = true; message.text = "Failed element"; diff --git a/capabilities2_events/include/capabilities2_events/event_listener.hpp b/capabilities2_events/include/capabilities2_events/event_listener.hpp new file mode 100644 index 0000000..9ebc501 --- /dev/null +++ b/capabilities2_events/include/capabilities2_events/event_listener.hpp @@ -0,0 +1,61 @@ +#include + +#include "rclcpp/rclcpp.hpp" +#include + +class CapabilitiesEventListener : public rclcpp::Node +{ +public: + using Event = capabilities2_msgs::msg::CapabilityEvent; + + CapabilitiesEventListener(const rclcpp::NodeOptions& options = rclcpp::NodeOptions()) : Node("capabilities2_events_listener", options) + { + // Create a subscription to the "topic" topic + RCLCPP_INFO(this->get_logger(), "Creating subscription to topic"); + + subscription_ = this->create_subscription("/events", 10, std::bind(&CapabilitiesEventListener::topic_callback, this, std::placeholders::_1)); + } + +private: + void topic_callback(const Event& msg) const + { + std::string text; + + if (msg.is_failed_element) + { + text = "[" + msg.origin_node + "]" + "[" + msg.source.capability + "/" + std::to_string(msg.thread_id) + "] " + msg.text + " : " + msg.element; + } + else if (msg.thread_id >= 0 and msg.target.capability == "" and msg.source.capability == "") + { + text = "[" + msg.origin_node + "]" + "[" + std::to_string(msg.thread_id) + "] " + msg.text; + } + else if (msg.thread_id < 0 and msg.target.capability == "" and msg.source.capability == "") + { + text = "[" + msg.origin_node + "] " + msg.text; + } + else if (msg.thread_id >= 0 and msg.target.capability == "" and msg.source.capability != "") + { + text = "[" + msg.origin_node + "]" + "[" + msg.source.capability + "/" + std::to_string(msg.thread_id) + "] " + msg.text; + } + else if (msg.thread_id < 0 and msg.target.capability == "" and msg.source.capability != "") + { + text = "[" + msg.origin_node + "]" + "[" + msg.source.capability + "] " + msg.text; + } + else if (msg.thread_id >= 0 and msg.target.capability != "") + { + text = "[" + msg.origin_node + "]" + "[" + msg.source.capability + "/" + std::to_string(msg.thread_id) + "] triggering " + + msg.target.capability + " " + msg.text; + } + else if (msg.thread_id < 0 and msg.target.capability != "") + { + text = "[" + msg.origin_node + "]" + "[" + msg.source.capability + "] triggering " + msg.target.capability + " " + msg.text; + } + + if (msg.error) + RCLCPP_ERROR(get_logger(), text.c_str()); + else + RCLCPP_INFO(get_logger(), text.c_str()); + } + + rclcpp::Subscription::SharedPtr subscription_; +}; \ No newline at end of file diff --git a/capabilities2_events/launch/listener.launch.py b/capabilities2_events/launch/listener.launch.py new file mode 100644 index 0000000..6e5a90e --- /dev/null +++ b/capabilities2_events/launch/listener.launch.py @@ -0,0 +1,28 @@ +''' +capabilities2_server launch file +''' + +import os +from launch import LaunchDescription +from launch_ros.actions import Node + + +def generate_launch_description(): + """Generate launch description for capabilities2 server + + Returns: + LaunchDescription: The launch description for capabilities2 events listener + """ + # create bridge composition + capabilities2 = Node( + package='capabilities2_events', + executable='capabilities2_events_node', + name='capabilities_events_listener', + output='screen', + arguments=['--ros-args', '--log-level', 'info'] + ) + + # return + return LaunchDescription([ + capabilities2 + ]) diff --git a/capabilities2_events/package.xml b/capabilities2_events/package.xml index 03d7c01..9674c1a 100644 --- a/capabilities2_events/package.xml +++ b/capabilities2_events/package.xml @@ -14,6 +14,7 @@ rclcpp capabilities2_msgs + rclcpp_components ament_cmake diff --git a/capabilities2_events/src/event_listener_node.cpp b/capabilities2_events/src/event_listener_node.cpp new file mode 100644 index 0000000..3eea375 --- /dev/null +++ b/capabilities2_events/src/event_listener_node.cpp @@ -0,0 +1,17 @@ +#include + +int main(int argc, char* argv[]) +{ + // Initialize the ROS 2 C++ client library + rclcpp::init(argc, argv); + + // Create a shared pointer to the CapabilitiesFabricClient + auto listener_node = std::make_shared(); + + // Spin the node to process callbacks + rclcpp::spin(listener_node); + + rclcpp::shutdown(); + + return 0; +} diff --git a/capabilities2_fabric/CMakeLists.txt b/capabilities2_fabric/CMakeLists.txt index 68c0273..5e3a2d2 100644 --- a/capabilities2_fabric/CMakeLists.txt +++ b/capabilities2_fabric/CMakeLists.txt @@ -54,7 +54,7 @@ install(TARGETS ${PROJECT_NAME} ############################################################################ add_library(${PROJECT_NAME}_comp SHARED - src/capabilities_fabric_comp.cpp + src/capabilities_fabric_component.cpp ) ament_target_dependencies(${PROJECT_NAME}_comp @@ -108,7 +108,7 @@ install(TARGETS fabric_client ############################################################################ add_library(fabric_client_comp SHARED - src/capabilities_fabric_client_comp.cpp + src/capabilities_fabric_client_component.cpp ) ament_target_dependencies(fabric_client_comp diff --git a/capabilities2_fabric/docs/prompt_capability_runner_ex1.md b/capabilities2_fabric/docs/prompt_capability_runner_ex1.md index b23e1c7..f1384e1 100644 --- a/capabilities2_fabric/docs/prompt_capability_runner_ex1.md +++ b/capabilities2_fabric/docs/prompt_capability_runner_ex1.md @@ -35,4 +35,11 @@ ros2 launch capabilities2_server server.launch.py ```bash source install/setup.bash ros2 launch capabilities2_fabric fabric.launch.py +``` + +### Start the Capabilities2 Event Listener + +```bash +source install/setup.bash +ros2 launch capabilities2_events listener.launch.py ``` \ No newline at end of file diff --git a/capabilities2_fabric/docs/prompt_occupancy_runner_ex1.md b/capabilities2_fabric/docs/prompt_occupancy_runner_ex1.md index 2489180..e98f101 100644 --- a/capabilities2_fabric/docs/prompt_occupancy_runner_ex1.md +++ b/capabilities2_fabric/docs/prompt_occupancy_runner_ex1.md @@ -49,4 +49,11 @@ ros2 launch capabilities2_server server.launch.py ```bash source install/setup.bash ros2 launch capabilities2_fabric fabric.launch.py +``` + +### Start the Capabilities2 Event Listener + +```bash +source install/setup.bash +ros2 launch capabilities2_events listener.launch.py ``` \ No newline at end of file diff --git a/capabilities2_fabric/docs/prompt_plan_runner_ex1.md b/capabilities2_fabric/docs/prompt_plan_runner_ex1.md index 01ee941..0867559 100644 --- a/capabilities2_fabric/docs/prompt_plan_runner_ex1.md +++ b/capabilities2_fabric/docs/prompt_plan_runner_ex1.md @@ -49,4 +49,11 @@ ros2 launch capabilities2_server server.launch.py ```bash source install/setup.bash ros2 launch capabilities2_fabric fabric.launch.py +``` + +### Start the Capabilities2 Event Listener + +```bash +source install/setup.bash +ros2 launch capabilities2_events listener.launch.py ``` \ No newline at end of file diff --git a/capabilities2_fabric/docs/prompt_pose_runner_ex1.md b/capabilities2_fabric/docs/prompt_pose_runner_ex1.md index 7c17a52..5d03dcd 100644 --- a/capabilities2_fabric/docs/prompt_pose_runner_ex1.md +++ b/capabilities2_fabric/docs/prompt_pose_runner_ex1.md @@ -49,4 +49,11 @@ ros2 launch capabilities2_server server.launch.py ```bash source install/setup.bash ros2 launch capabilities2_fabric fabric.launch.py +``` + +### Start the Capabilities2 Event Listener + +```bash +source install/setup.bash +ros2 launch capabilities2_events listener.launch.py ``` \ No newline at end of file diff --git a/capabilities2_fabric/docs/waypoint_runner_ex1.md b/capabilities2_fabric/docs/waypoint_runner_ex1.md index 5e199ef..3471863 100644 --- a/capabilities2_fabric/docs/waypoint_runner_ex1.md +++ b/capabilities2_fabric/docs/waypoint_runner_ex1.md @@ -42,4 +42,11 @@ ros2 launch capabilities2_server server.launch.py ```bash source install/setup.bash ros2 launch capabilities2_fabric fabric.launch.py +``` + +### Start the Capabilities2 Event Listener + +```bash +source install/setup.bash +ros2 launch capabilities2_events listener.launch.py ``` \ No newline at end of file diff --git a/capabilities2_fabric/docs/waypoint_runner_ex2.md b/capabilities2_fabric/docs/waypoint_runner_ex2.md index 871b296..0f63880 100644 --- a/capabilities2_fabric/docs/waypoint_runner_ex2.md +++ b/capabilities2_fabric/docs/waypoint_runner_ex2.md @@ -42,4 +42,11 @@ ros2 launch capabilities2_server server.launch.py ```bash source install/setup.bash ros2 launch capabilities2_fabric fabric.launch.py +``` + +### Start the Capabilities2 Event Listener + +```bash +source install/setup.bash +ros2 launch capabilities2_events listener.launch.py ``` \ No newline at end of file diff --git a/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric.hpp b/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric.hpp index 4e2c7fe..2e8bf8c 100644 --- a/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric.hpp +++ b/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric.hpp @@ -82,7 +82,7 @@ class CapabilitiesFabric : public rclcpp::Node { control_tag_list = xml_parser::get_control_list(); - event_ = std::make_shared(shared_from_this(), "capabilities_fabric", "/events/capabilities_fabric"); + event_ = std::make_shared(shared_from_this(), "capabilities_fabric", "/events"); this->planner_server_ = rclcpp_action::create_server( this, "/capabilities_fabric", std::bind(&CapabilitiesFabric::handle_goal, this, std::placeholders::_1, std::placeholders::_2), diff --git a/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric_client.hpp b/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric_client.hpp index ebd1fff..1745425 100644 --- a/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric_client.hpp +++ b/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric_client.hpp @@ -77,7 +77,7 @@ class CapabilitiesFabricClient : public rclcpp::Node fabric_state = Status::IDLE; - event_ = std::make_shared(shared_from_this(), "capabilities_fabric_client", "/events/capabilities_fabric_client"); + event_ = std::make_shared(shared_from_this(), "capabilities_fabric_client", "/events"); status_server_ = this->create_service("/capabilities_fabric/get_status", std::bind(&CapabilitiesFabricClient::getStatusCallback, this, diff --git a/capabilities2_fabric/src/capabilities_fabric_client_comp.cpp b/capabilities2_fabric/src/capabilities_fabric_client_component.cpp similarity index 100% rename from capabilities2_fabric/src/capabilities_fabric_client_comp.cpp rename to capabilities2_fabric/src/capabilities_fabric_client_component.cpp diff --git a/capabilities2_fabric/src/capabilities_fabric_client_node.cpp b/capabilities2_fabric/src/capabilities_fabric_client_node.cpp index 9bec80e..f10f3c9 100644 --- a/capabilities2_fabric/src/capabilities_fabric_client_node.cpp +++ b/capabilities2_fabric/src/capabilities_fabric_client_node.cpp @@ -2,13 +2,18 @@ int main(int argc, char* argv[]) { + // Initialize the ROS 2 C++ client library rclcpp::init(argc, argv); + // Create a shared pointer to the CapabilitiesFabricClient auto parser_node = std::make_shared(); + // Initialize the node parser_node->initialize(); // Call initialize after construction + // Spin the node to process callbacks rclcpp::spin(parser_node); + rclcpp::shutdown(); return 0; diff --git a/capabilities2_fabric/src/capabilities_fabric_comp.cpp b/capabilities2_fabric/src/capabilities_fabric_component.cpp similarity index 100% rename from capabilities2_fabric/src/capabilities_fabric_comp.cpp rename to capabilities2_fabric/src/capabilities_fabric_component.cpp diff --git a/capabilities2_runner/CMakeLists.txt b/capabilities2_runner/CMakeLists.txt index 2c985d7..d866eee 100644 --- a/capabilities2_runner/CMakeLists.txt +++ b/capabilities2_runner/CMakeLists.txt @@ -10,10 +10,6 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() -include_directories( - include -) - find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_action REQUIRED) @@ -23,6 +19,10 @@ find_package(capabilities2_events REQUIRED) find_package(tinyxml2_vendor REQUIRED) find_package(TinyXML2 REQUIRED) # provided by tinyxml2 upstream, or tinyxml2_vendor +include_directories( + include +) + add_library(${PROJECT_NAME} SHARED src/standard_runners.cpp ) diff --git a/capabilities2_runner/include/capabilities2_runner/runner_base.hpp b/capabilities2_runner/include/capabilities2_runner/runner_base.hpp index 29cbf29..729e663 100644 --- a/capabilities2_runner/include/capabilities2_runner/runner_base.hpp +++ b/capabilities2_runner/include/capabilities2_runner/runner_base.hpp @@ -131,7 +131,7 @@ class RunnerBase execute_id = -1; thread_id = 0; - event_ = std::make_shared(node_, "capability_runners", "/events/capability_runners"); + event_ = std::make_shared(node_, "capability_runners", "/events"); } /** diff --git a/capabilities2_server/CMakeLists.txt b/capabilities2_server/CMakeLists.txt index 05e7d1a..384014b 100644 --- a/capabilities2_server/CMakeLists.txt +++ b/capabilities2_server/CMakeLists.txt @@ -14,9 +14,9 @@ endif() find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_action REQUIRED) +find_package(rclcpp_components REQUIRED) find_package(bondcpp REQUIRED) find_package(pluginlib REQUIRED) -find_package(rclcpp_components REQUIRED) find_package(capabilities2_msgs REQUIRED) find_package(capabilities2_runner REQUIRED) find_package(capabilities2_events REQUIRED) @@ -43,17 +43,17 @@ include_directories(include # Component based implementation that compiles as a library ####################################################################### -add_library(${PROJECT_NAME} SHARED - src/capabilities_server_comp.cpp +add_library(${PROJECT_NAME}_comp SHARED + src/capabilities_server_component.cpp ) -target_link_libraries(${PROJECT_NAME} +target_link_libraries(${PROJECT_NAME}_comp ${SQLITE3_LIBRARIES} ${YAML_CPP_LIBRARIES} ${UUID_LIBRARIES} ) -ament_target_dependencies(${PROJECT_NAME} +ament_target_dependencies(${PROJECT_NAME}_comp rclcpp rclcpp_action bondcpp @@ -68,15 +68,15 @@ ament_target_dependencies(${PROJECT_NAME} UUID ) -rclcpp_components_register_node(${PROJECT_NAME} +rclcpp_components_register_node(${PROJECT_NAME}_comp PLUGIN "capabilities2_server::CapabilitiesServer" - EXECUTABLE capabilities2_server_component_node + EXECUTABLE capabilities2_server_component ) -ament_export_targets(capabilities2_server_component_node) +ament_export_targets(capabilities2_server_component) -install(TARGETS ${PROJECT_NAME} - EXPORT capabilities2_server_component_node +install(TARGETS ${PROJECT_NAME}_comp + EXPORT capabilities2_server_component ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin @@ -101,7 +101,6 @@ ament_target_dependencies(${PROJECT_NAME}_node rclcpp_action bondcpp pluginlib - rclcpp_components capabilities2_msgs capabilities2_runner capabilities2_events @@ -118,13 +117,14 @@ install(TARGETS ${PROJECT_NAME}_node # make test executable add_executable(test_capabilities_server test/test.cpp) + target_link_libraries(test_capabilities_server - ${PROJECT_NAME} + ${PROJECT_NAME}_comp ${SQLITE3_LIBRARIES} ${YAML_CPP_LIBRARIES} ${UUID_LIBRARIES} ) -add_dependencies(test_capabilities_server ${PROJECT_NAME}) +add_dependencies(test_capabilities_server ${PROJECT_NAME}_comp) ############################################################################ # miscellaneous diff --git a/capabilities2_server/include/capabilities2_server/capabilities_server.hpp b/capabilities2_server/include/capabilities2_server/capabilities_server.hpp index 3570fd1..71e4e56 100644 --- a/capabilities2_server/include/capabilities2_server/capabilities_server.hpp +++ b/capabilities2_server/include/capabilities2_server/capabilities_server.hpp @@ -84,7 +84,7 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI void initialize() { // pubs - event_ = std::make_shared(shared_from_this(), "capabilities_server", "/events/capabilities_server"); + event_ = std::make_shared(shared_from_this(), "capabilities_server", "/events"); // params interface // loop rate @@ -128,7 +128,7 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI } // init capabilities api - event_->info("Connecting API with Database"); + event_->info("Connecting Capabilities API with Database"); connect(db_file, event_); @@ -442,7 +442,7 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI private: void load_capabilities(const std::string& package_path) { - event_->info("Loading capabilities from package path: " + package_path); + // event_->info("Loading capabilities from package path: " + package_path); // check if path exists if (!std::filesystem::exists(package_path)) @@ -474,7 +474,7 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI // load capabilities from packages in /opt/ros/*/share for (const auto& package : packages_root) { - event_->info("Loading capabilities from package: " + package); + // event_->info("Loading capabilities from package: " + package); // package.xml exports std::string package_xml = package_path + "/" + package + "/package.xml"; @@ -557,7 +557,7 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI // load capabilities from packages in workspace install folder for (const auto& package : packages_install) { - event_->info("Loading capabilities from package: " + package); + // event_->info("Loading capabilities from package: " + package); // package.xml exports std::string package_xml = package_path + "/" + package + "/share/" + package + "/package.xml"; diff --git a/capabilities2_server/src/capabilities_server_comp.cpp b/capabilities2_server/src/capabilities_server_component.cpp similarity index 100% rename from capabilities2_server/src/capabilities_server_comp.cpp rename to capabilities2_server/src/capabilities_server_component.cpp From 66e7505a5b6a5ea8fa3d62beacaf87ac3b1109fb Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Wed, 7 May 2025 00:34:30 +1000 Subject: [PATCH 106/133] updated event msg structure --- .../capabilities2_events/event_client.hpp | 64 +++++++++++++++---- .../capabilities2_events/event_listener.hpp | 27 ++++---- .../launch/listener.launch.py | 2 +- .../docs/prompt_capability_runner_ex1.md | 2 +- .../docs/prompt_occupancy_runner_ex1.md | 2 +- .../docs/prompt_plan_runner_ex1.md | 2 +- .../docs/prompt_pose_runner_ex1.md | 2 +- .../docs/waypoint_runner_ex1.md | 2 +- .../docs/waypoint_runner_ex2.md | 2 +- .../capabilities_fabric.hpp | 2 +- .../capabilities_fabric_client.hpp | 2 +- capabilities2_msgs/msg/CapabilityEvent.msg | 22 +++---- .../capabilities2_runner/runner_base.hpp | 22 +++---- .../capabilities2_server/capabilities_api.hpp | 2 +- .../capabilities_server.hpp | 8 +-- 15 files changed, 101 insertions(+), 62 deletions(-) diff --git a/capabilities2_events/include/capabilities2_events/event_client.hpp b/capabilities2_events/include/capabilities2_events/event_client.hpp index 9ff3f79..742a262 100644 --- a/capabilities2_events/include/capabilities2_events/event_client.hpp +++ b/capabilities2_events/include/capabilities2_events/event_client.hpp @@ -43,10 +43,8 @@ class EventClient message.target.provider = ""; message.thread_id = thread_id; message.event = Event::UNDEFINED; - message.error = false; - message.text = text; - message.is_failed_element = false; - message.element = ""; + message.type = Event::INFO; + message.content = text; message.pid = -1; event_publisher_->publish(message); @@ -62,6 +60,41 @@ class EventClient event_publisher_->publish(message); } + /** + * @brief publishes status information to the given topic as debug + * + * @param text Text to be published + */ + + void debug(const std::string& text, int thread_id = -1) + { + auto message = Event(); + + message.header.stamp = node_->now(); + message.origin_node = node_name_; + message.source.capability = ""; + message.source.provider = ""; + message.target.capability = ""; + message.target.provider = ""; + message.thread_id = thread_id; + message.event = Event::UNDEFINED; + message.type = Event::DEBUG; + message.content = text; + message.pid = -1; + + event_publisher_->publish(message); + } + + /** + * @brief publishes status information to the given topic as debug + * + * @param message Message to be published + */ + void debug(const Event& message) + { + event_publisher_->publish(message); + } + /** * @brief publishes status information to the given topic as error * @@ -79,10 +112,8 @@ class EventClient message.target.provider = ""; message.thread_id = thread_id; message.event = Event::UNDEFINED; - message.error = true; - message.text = text; - message.is_failed_element = false; - message.element = ""; + message.type = Event::ERROR; + message.content = text; message.pid = -1; event_publisher_->publish(message); @@ -115,15 +146,24 @@ class EventClient message.target.provider = ""; message.thread_id = thread_id; message.event = Event::UNDEFINED; - message.error = true; - message.text = "Failed element"; - message.is_failed_element = true; - message.element = element; + message.type = Event::ERROR_ELEMENT; + message.content = element; message.pid = -1; event_publisher_->publish(message); } + /** + * @brief publishes status information to the given topic as error with + * element infromation included + * + * @param message Message to be published + */ + void error_element(const Event& message) + { + event_publisher_->publish(message); + } + protected: /** * @brief Node pointer to access logging interface diff --git a/capabilities2_events/include/capabilities2_events/event_listener.hpp b/capabilities2_events/include/capabilities2_events/event_listener.hpp index 9ebc501..4099719 100644 --- a/capabilities2_events/include/capabilities2_events/event_listener.hpp +++ b/capabilities2_events/include/capabilities2_events/event_listener.hpp @@ -13,7 +13,8 @@ class CapabilitiesEventListener : public rclcpp::Node // Create a subscription to the "topic" topic RCLCPP_INFO(this->get_logger(), "Creating subscription to topic"); - subscription_ = this->create_subscription("/events", 10, std::bind(&CapabilitiesEventListener::topic_callback, this, std::placeholders::_1)); + subscription_ = + this->create_subscription("/events", 10, std::bind(&CapabilitiesEventListener::topic_callback, this, std::placeholders::_1)); } private: @@ -21,40 +22,44 @@ class CapabilitiesEventListener : public rclcpp::Node { std::string text; - if (msg.is_failed_element) + if (msg.type == Event::ERROR_ELEMENT) { - text = "[" + msg.origin_node + "]" + "[" + msg.source.capability + "/" + std::to_string(msg.thread_id) + "] " + msg.text + " : " + msg.element; + text = "[" + msg.origin_node + "]" + "[" + msg.source.capability + "/" + std::to_string(msg.thread_id) + "] " + msg.content; } else if (msg.thread_id >= 0 and msg.target.capability == "" and msg.source.capability == "") { - text = "[" + msg.origin_node + "]" + "[" + std::to_string(msg.thread_id) + "] " + msg.text; + text = "[" + msg.origin_node + "]" + "[" + std::to_string(msg.thread_id) + "] " + msg.content; } else if (msg.thread_id < 0 and msg.target.capability == "" and msg.source.capability == "") { - text = "[" + msg.origin_node + "] " + msg.text; + text = "[" + msg.origin_node + "] " + msg.content; } else if (msg.thread_id >= 0 and msg.target.capability == "" and msg.source.capability != "") { - text = "[" + msg.origin_node + "]" + "[" + msg.source.capability + "/" + std::to_string(msg.thread_id) + "] " + msg.text; + text = "[" + msg.origin_node + "]" + "[" + msg.source.capability + "/" + std::to_string(msg.thread_id) + "] " + msg.content; } else if (msg.thread_id < 0 and msg.target.capability == "" and msg.source.capability != "") { - text = "[" + msg.origin_node + "]" + "[" + msg.source.capability + "] " + msg.text; + text = "[" + msg.origin_node + "]" + "[" + msg.source.capability + "] " + msg.content; } else if (msg.thread_id >= 0 and msg.target.capability != "") { text = "[" + msg.origin_node + "]" + "[" + msg.source.capability + "/" + std::to_string(msg.thread_id) + "] triggering " + - msg.target.capability + " " + msg.text; + msg.target.capability + " " + msg.content; } else if (msg.thread_id < 0 and msg.target.capability != "") { - text = "[" + msg.origin_node + "]" + "[" + msg.source.capability + "] triggering " + msg.target.capability + " " + msg.text; + text = "[" + msg.origin_node + "]" + "[" + msg.source.capability + "] triggering " + msg.target.capability + " " + msg.content; } - if (msg.error) + if (msg.type == Event::ERROR) RCLCPP_ERROR(get_logger(), text.c_str()); - else + else if (msg.type == Event::INFO) RCLCPP_INFO(get_logger(), text.c_str()); + else if (msg.type == Event::DEBUG) + RCLCPP_DEBUG(get_logger(), text.c_str()); + else + RCLCPP_ERROR(get_logger(), text.c_str()); } rclcpp::Subscription::SharedPtr subscription_; diff --git a/capabilities2_events/launch/listener.launch.py b/capabilities2_events/launch/listener.launch.py index 6e5a90e..3f5951f 100644 --- a/capabilities2_events/launch/listener.launch.py +++ b/capabilities2_events/launch/listener.launch.py @@ -17,7 +17,7 @@ def generate_launch_description(): capabilities2 = Node( package='capabilities2_events', executable='capabilities2_events_node', - name='capabilities_events_listener', + name='listener', output='screen', arguments=['--ros-args', '--log-level', 'info'] ) diff --git a/capabilities2_fabric/docs/prompt_capability_runner_ex1.md b/capabilities2_fabric/docs/prompt_capability_runner_ex1.md index f1384e1..0028ccb 100644 --- a/capabilities2_fabric/docs/prompt_capability_runner_ex1.md +++ b/capabilities2_fabric/docs/prompt_capability_runner_ex1.md @@ -37,7 +37,7 @@ source install/setup.bash ros2 launch capabilities2_fabric fabric.launch.py ``` -### Start the Capabilities2 Event Listener +### Start the Capabilities2 Event Listener (Optional for Debugging) ```bash source install/setup.bash diff --git a/capabilities2_fabric/docs/prompt_occupancy_runner_ex1.md b/capabilities2_fabric/docs/prompt_occupancy_runner_ex1.md index e98f101..52c6ea3 100644 --- a/capabilities2_fabric/docs/prompt_occupancy_runner_ex1.md +++ b/capabilities2_fabric/docs/prompt_occupancy_runner_ex1.md @@ -51,7 +51,7 @@ source install/setup.bash ros2 launch capabilities2_fabric fabric.launch.py ``` -### Start the Capabilities2 Event Listener +### Start the Capabilities2 Event Listener (Optional for Debugging) ```bash source install/setup.bash diff --git a/capabilities2_fabric/docs/prompt_plan_runner_ex1.md b/capabilities2_fabric/docs/prompt_plan_runner_ex1.md index 0867559..bdd8b30 100644 --- a/capabilities2_fabric/docs/prompt_plan_runner_ex1.md +++ b/capabilities2_fabric/docs/prompt_plan_runner_ex1.md @@ -51,7 +51,7 @@ source install/setup.bash ros2 launch capabilities2_fabric fabric.launch.py ``` -### Start the Capabilities2 Event Listener +### Start the Capabilities2 Event Listener (Optional for Debugging) ```bash source install/setup.bash diff --git a/capabilities2_fabric/docs/prompt_pose_runner_ex1.md b/capabilities2_fabric/docs/prompt_pose_runner_ex1.md index 5d03dcd..d572598 100644 --- a/capabilities2_fabric/docs/prompt_pose_runner_ex1.md +++ b/capabilities2_fabric/docs/prompt_pose_runner_ex1.md @@ -51,7 +51,7 @@ source install/setup.bash ros2 launch capabilities2_fabric fabric.launch.py ``` -### Start the Capabilities2 Event Listener +### Start the Capabilities2 Event Listener (Optional for Debugging) ```bash source install/setup.bash diff --git a/capabilities2_fabric/docs/waypoint_runner_ex1.md b/capabilities2_fabric/docs/waypoint_runner_ex1.md index 3471863..ef0d785 100644 --- a/capabilities2_fabric/docs/waypoint_runner_ex1.md +++ b/capabilities2_fabric/docs/waypoint_runner_ex1.md @@ -44,7 +44,7 @@ source install/setup.bash ros2 launch capabilities2_fabric fabric.launch.py ``` -### Start the Capabilities2 Event Listener +### Start the Capabilities2 Event Listener (Optional for Debugging) ```bash source install/setup.bash diff --git a/capabilities2_fabric/docs/waypoint_runner_ex2.md b/capabilities2_fabric/docs/waypoint_runner_ex2.md index 0f63880..b13df68 100644 --- a/capabilities2_fabric/docs/waypoint_runner_ex2.md +++ b/capabilities2_fabric/docs/waypoint_runner_ex2.md @@ -44,7 +44,7 @@ source install/setup.bash ros2 launch capabilities2_fabric fabric.launch.py ``` -### Start the Capabilities2 Event Listener +### Start the Capabilities2 Event Listener (Optional for Debugging) ```bash source install/setup.bash diff --git a/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric.hpp b/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric.hpp index 2e8bf8c..f82a108 100644 --- a/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric.hpp +++ b/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric.hpp @@ -82,7 +82,7 @@ class CapabilitiesFabric : public rclcpp::Node { control_tag_list = xml_parser::get_control_list(); - event_ = std::make_shared(shared_from_this(), "capabilities_fabric", "/events"); + event_ = std::make_shared(shared_from_this(), "fabric", "/events"); this->planner_server_ = rclcpp_action::create_server( this, "/capabilities_fabric", std::bind(&CapabilitiesFabric::handle_goal, this, std::placeholders::_1, std::placeholders::_2), diff --git a/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric_client.hpp b/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric_client.hpp index 1745425..1434d0a 100644 --- a/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric_client.hpp +++ b/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric_client.hpp @@ -77,7 +77,7 @@ class CapabilitiesFabricClient : public rclcpp::Node fabric_state = Status::IDLE; - event_ = std::make_shared(shared_from_this(), "capabilities_fabric_client", "/events"); + event_ = std::make_shared(shared_from_this(), "client", "/events"); status_server_ = this->create_service("/capabilities_fabric/get_status", std::bind(&CapabilitiesFabricClient::getStatusCallback, this, diff --git a/capabilities2_msgs/msg/CapabilityEvent.msg b/capabilities2_msgs/msg/CapabilityEvent.msg index 9136623..cd7d38b 100644 --- a/capabilities2_msgs/msg/CapabilityEvent.msg +++ b/capabilities2_msgs/msg/CapabilityEvent.msg @@ -12,7 +12,7 @@ Capability target # Thread id of the capability which this event pertains to int8 thread_id -# Event types available +# Events available uint8 IDLE=0 uint8 STARTED=1 uint8 STOPPED=2 @@ -20,20 +20,20 @@ uint8 FAILED=3 uint8 SUCCEEDED=4 uint8 UNDEFINED=5 -# Specific event that occurred +# Related event uint8 event -# Whether this error relates to a failure -bool error - -# status message -string text +# Event types available +uint8 INFO=0 +uint8 DEBUG=1 +uint8 ERROR=2 +uint8 ERROR_ELEMENT=3 -# Whether output data containes xml elements -bool is_failed_element +# Related event type +uint8 type -# string contianing xml elements -string element +# status message +string content # PID of the related process int32 pid diff --git a/capabilities2_runner/include/capabilities2_runner/runner_base.hpp b/capabilities2_runner/include/capabilities2_runner/runner_base.hpp index 729e663..2c9bbdf 100644 --- a/capabilities2_runner/include/capabilities2_runner/runner_base.hpp +++ b/capabilities2_runner/include/capabilities2_runner/runner_base.hpp @@ -131,7 +131,7 @@ class RunnerBase execute_id = -1; thread_id = 0; - event_ = std::make_shared(node_, "capability_runners", "/events"); + event_ = std::make_shared(node_, "runner", "/events"); } /** @@ -459,10 +459,8 @@ class RunnerBase message.target.capability = target_capability; message.target.provider = target_provider; message.thread_id = thread_id; - message.error = false; - message.text = text; - message.is_failed_element = false; - message.element = ""; + message.type = Event::INFO; + message.content = text; message.pid = -1; switch (event) @@ -501,10 +499,8 @@ class RunnerBase message.target.capability = ""; message.target.provider = ""; message.thread_id = thread_id; - message.error = true; - message.text = text; - message.is_failed_element = false; - message.element = ""; + message.type = Event::ERROR; + message.content = text; message.pid = -1; message.event = Event::UNDEFINED; @@ -522,14 +518,12 @@ class RunnerBase message.target.capability = ""; message.target.provider = ""; message.thread_id = thread_id; - message.error = true; - message.text = text; - message.is_failed_element = true; - message.element = element; + message.type = Event::ERROR_ELEMENT; + message.content = text + " : " + element; message.pid = -1; message.event = Event::UNDEFINED; - event_->error(message); + event_->error_element(message); } /** diff --git a/capabilities2_server/include/capabilities2_server/capabilities_api.hpp b/capabilities2_server/include/capabilities2_server/capabilities_api.hpp index 9717d1a..7bab86a 100644 --- a/capabilities2_server/include/capabilities2_server/capabilities_api.hpp +++ b/capabilities2_server/include/capabilities2_server/capabilities_api.hpp @@ -612,7 +612,7 @@ class CapabilitiesAPI void on_bond_established(const std::string& bond_id) { // log bond established event - event_->error("bond established with id: " + bond_id); + event_->info("bond established with id: " + bond_id); } void on_bond_broken(const std::string& bond_id) diff --git a/capabilities2_server/include/capabilities2_server/capabilities_server.hpp b/capabilities2_server/include/capabilities2_server/capabilities_server.hpp index 71e4e56..f177673 100644 --- a/capabilities2_server/include/capabilities2_server/capabilities_server.hpp +++ b/capabilities2_server/include/capabilities2_server/capabilities_server.hpp @@ -84,7 +84,7 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI void initialize() { // pubs - event_ = std::make_shared(shared_from_this(), "capabilities_server", "/events"); + event_ = std::make_shared(shared_from_this(), "server", "/events"); // params interface // loop rate @@ -442,7 +442,7 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI private: void load_capabilities(const std::string& package_path) { - // event_->info("Loading capabilities from package path: " + package_path); + event_->debug("Loading capabilities from package path: " + package_path); // check if path exists if (!std::filesystem::exists(package_path)) @@ -474,7 +474,7 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI // load capabilities from packages in /opt/ros/*/share for (const auto& package : packages_root) { - // event_->info("Loading capabilities from package: " + package); + event_->debug("Loading capabilities from package: " + package); // package.xml exports std::string package_xml = package_path + "/" + package + "/package.xml"; @@ -557,7 +557,7 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI // load capabilities from packages in workspace install folder for (const auto& package : packages_install) { - // event_->info("Loading capabilities from package: " + package); + event_->debug("Loading capabilities from package: " + package); // package.xml exports std::string package_xml = package_path + "/" + package + "/share/" + package + "/package.xml"; From f0a5ea923b246d367c4fe6085f124fe9db9533ba Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Wed, 7 May 2025 13:32:40 +1000 Subject: [PATCH 107/133] completed event system testing --- .../capabilities_fabric.hpp | 84 ++++++++----------- .../capabilities2_fabric/utils/xml_parser.hpp | 57 ++++++------- 2 files changed, 60 insertions(+), 81 deletions(-) diff --git a/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric.hpp b/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric.hpp index f82a108..6372066 100644 --- a/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric.hpp +++ b/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric.hpp @@ -391,8 +391,41 @@ class CapabilitiesFabric : public rclcpp::Node { event_->info("Verifying the plan"); + bool verification_success = true; + + auto result = std::make_shared(); + + // extract the components within the 'plan' tags + bool extraction_success = false; + tinyxml2::XMLElement* plan = xml_parser::get_plan(document, extraction_success); + + if (!extraction_success) + { + result_msg->success = false; + result_msg->message = "Execution plan is not compatible. Please recheck and update"; + event_->error(result_msg->message); + goal_handle_->abort(result_msg); + verification_success = false; + } + + event_->info("Plan extraction complete"); + + // verify whether the plan is valid by checking the tags + std::string error_message; + + if (!xml_parser::check_tags(plan, interface_list, providers_list, control_tag_list, rejected_list, error_message)) + { + result_msg->success = false; + result_msg->message = "Execution plan is faulty. Please recheck and update"; + event_->error(result_msg->message); + goal_handle_->abort(result_msg); + verification_success = false; + } + + event_->info("Checking tags successful"); + // verify the plan - if (!verify_plan()) + if (!verification_success) { event_->info("Plan verification failed"); @@ -407,8 +440,8 @@ class CapabilitiesFabric : public rclcpp::Node { result->failed_elements.push_back(rejected_element); } - goal_handle_->abort(result); + goal_handle_->abort(result); event_->info(result->message); } else @@ -424,12 +457,7 @@ class CapabilitiesFabric : public rclcpp::Node event_->error("Server Execution Cancelled"); } - event_->info("Plan verification successful"); - - // extract the plan from the XMLDocument - tinyxml2::XMLElement* plan = xml_parser::get_plan(document); - - event_->info("Plan conversion successful"); + event_->info("Plan verification successful. Proceeding with connections extraction"); // Extract the connections from the plan xml_parser::extract_connections(plan, connection_map); @@ -440,46 +468,6 @@ class CapabilitiesFabric : public rclcpp::Node request_bond(); } - /** - * @brief verify the plan using received interfaces - * - * @return `true` if interface retreival is successful,`false` otherwise - */ - bool verify_plan() - { - auto result = std::make_shared(); - - // verify whether document got 'plan' tags - if (!xml_parser::check_plan_tag(document)) - { - result_msg->success = false; - result_msg->message = "Execution plan is not compatible. Please recheck and update"; - event_->error(result_msg->message); - goal_handle_->abort(result_msg); - return false; - } - - event_->info("'Plan' tag checking successful"); - - // extract the components within the 'plan' tags - tinyxml2::XMLElement* plan = xml_parser::get_plan(document); - - event_->info("Plan extraction complete"); - - // verify whether the plan is valid - if (!xml_parser::check_tags(event_, plan, interface_list, providers_list, control_tag_list, rejected_list)) - { - result_msg->success = false; - result_msg->message = "Execution plan is faulty. Please recheck and update"; - event_->error(result_msg->message); - goal_handle_->abort(result_msg); - return false; - } - - event_->info("Checking tags successful"); - return true; - } - /** * @brief Request the bond from the capabilities2 server * diff --git a/capabilities2_fabric/include/capabilities2_fabric/utils/xml_parser.hpp b/capabilities2_fabric/include/capabilities2_fabric/utils/xml_parser.hpp index 4e64e79..d3b8e92 100644 --- a/capabilities2_fabric/include/capabilities2_fabric/utils/xml_parser.hpp +++ b/capabilities2_fabric/include/capabilities2_fabric/utils/xml_parser.hpp @@ -9,15 +9,27 @@ namespace xml_parser { /** - * @brief extract elements related plan + * @brief extract elements related plan and return the first child element * * @param document XML document to extract plan from + * @param success boolean to indicate if the plan was found * * @return plan in the form of tinyxml2::XMLElement* */ -tinyxml2::XMLElement* get_plan(tinyxml2::XMLDocument& document) +tinyxml2::XMLElement* get_plan(tinyxml2::XMLDocument& document, bool& success) { - return document.FirstChildElement("Plan")->FirstChildElement(); + std::string plan_tag(document.FirstChildElement()->Name()); + + if (plan_tag == "Plan") + { + success = true; + return document.FirstChildElement("Plan")->FirstChildElement(); + } + else + { + success = false; + return nullptr; + } } /** @@ -33,23 +45,6 @@ bool search(std::vector list, std::string value) return (std::find(list.begin(), list.end(), value) != list.end()); } -/** - * @brief check if the xml document has valid plan tags - * - * @param document XML document in question - * - * @return `true` if its valid and `false` otherwise - */ -bool check_plan_tag(tinyxml2::XMLDocument& document) -{ - std::string plan_tag(document.FirstChildElement()->Name()); - - if (plan_tag == "Plan") - return true; - else - return false; -} - /** * @brief convert XMLElement to std::string * @@ -132,8 +127,8 @@ void add_closing_event(tinyxml2::XMLDocument& document) * * @return `true` if element valid and supported and `false` otherwise */ -bool check_tags(const std::shared_ptr event, tinyxml2::XMLElement* element, std::vector& events, - std::vector& providers, std::vector& control, std::vector& rejected) +bool check_tags(tinyxml2::XMLElement* element, std::vector& events, std::vector& providers, + std::vector& control, std::vector& rejected, std::string& error) { const char* name; const char* provider; @@ -146,7 +141,6 @@ bool check_tags(const std::shared_ptr event, tinyxml2::XMLElement* std::string nametag; std::string providertag; - std::string typetag(element->Name()); if (name) @@ -170,35 +164,32 @@ bool check_tags(const std::shared_ptr event, tinyxml2::XMLElement* { if (!foundInControl) { - std::string msg = "Control tag '" + nametag + "' not available in the valid list"; - event->error(msg); + error = "Control tag '" + nametag + "' not available in the valid list"; rejected.push_back(parameter_string); return false; } if (hasChildren) - returnValue &= xml_parser::check_tags(event, element->FirstChildElement(), events, providers, control, rejected); + returnValue &= xml_parser::check_tags(element->FirstChildElement(), events, providers, control, rejected, error); if (hasSiblings) - returnValue &= xml_parser::check_tags(event, element->NextSiblingElement(), events, providers, control, rejected); + returnValue &= xml_parser::check_tags(element->NextSiblingElement(), events, providers, control, rejected, error); } else if (typetag == "Event") { if (!foundInEvents || !foundInProviders) { - std::string msg = "Event tag name '" + nametag + "' or provider '" + providertag + "' not available in the valid list"; - event->error(msg); + error = "Event tag name '" + nametag + "' or provider '" + providertag + "' not available in the valid list"; rejected.push_back(parameter_string); return false; } if (hasSiblings) - returnValue &= xml_parser::check_tags(event, element->NextSiblingElement(), events, providers, control, rejected); + returnValue &= xml_parser::check_tags(element->NextSiblingElement(), events, providers, control, rejected, error); } else { - std::string msg = "XML element is not valid :" + parameter_string; - event->error(msg); + error = "XML element is not valid :" + parameter_string; rejected.push_back(parameter_string); return false; } @@ -278,7 +269,7 @@ int extract_connections(tinyxml2::XMLElement* element, std::mapNextSiblingElement(), connections, predecessor_id+1, connection_type); + predecessor_id = xml_parser::extract_connections(element->NextSiblingElement(), connections, predecessor_id + 1, connection_type); } return predecessor_id; From d562129448133d7309c5471d5ffe10c53d83be87 Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Wed, 7 May 2025 15:10:39 +1000 Subject: [PATCH 108/133] moved fabric related functionality to a new repo --- capabilities2_fabric/CMakeLists.txt | 159 ---- capabilities2_fabric/config/fabric.yaml | 9 - .../docs/prompt_capability_runner_ex1.md | 45 - .../docs/prompt_occupancy_runner_ex1.md | 59 -- .../docs/prompt_plan_runner_ex1.md | 59 -- .../docs/prompt_pose_runner_ex1.md | 59 -- .../docs/waypoint_runner_ex1.md | 52 -- .../docs/waypoint_runner_ex2.md | 52 -- .../capabilities_fabric.hpp | 874 ------------------ .../capabilities_fabric_client.hpp | 359 ------- .../capabilities2_fabric/utils/xml_parser.hpp | 313 ------- capabilities2_fabric/launch/fabric.launch.py | 40 - .../launch/fabric_composed.launch.py | 49 - capabilities2_fabric/plans/default.xml | 13 - capabilities2_fabric/plans/navigation_1.xml | 6 - capabilities2_fabric/plans/navigation_2.xml | 7 - capabilities2_fabric/plans/prompt_1.xml | 7 - capabilities2_fabric/plans/prompt_2.xml | 7 - capabilities2_fabric/plans/prompt_3.xml | 8 - capabilities2_fabric/plans/prompt_4.xml | 9 - capabilities2_fabric/readme.md | 102 -- .../capabilities_fabric_client_component.cpp | 4 - .../src/capabilities_fabric_client_node.cpp | 20 - .../src/capabilities_fabric_component.cpp | 4 - .../src/capabilities_fabric_node.cpp | 17 - capabilities2_msgs/CMakeLists.txt | 5 - capabilities2_msgs/action/Plan.action | 14 - capabilities2_msgs/srv/CancelFabricPlan.srv | 4 - capabilities2_msgs/srv/CompleteFabric.srv | 1 - capabilities2_msgs/srv/GetFabricStatus.srv | 12 - capabilities2_msgs/srv/SetFabricPlan.srv | 5 - ...pability_get_runner.hpp => get_runner.hpp} | 0 capabilities2_runner_capabilities/plugins.xml | 10 - .../src/capabilities2_runner.cpp | 6 +- .../.clang-format | 2 +- capabilities2_runner_fabric/CMakeLists.txt | 59 ++ .../completion_runner.hpp | 10 +- .../set_plan_runner.hpp | 10 +- .../package.xml | 18 +- capabilities2_runner_fabric/plugins.xml | 12 + .../src/fabric_runners.cpp | 8 + 41 files changed, 100 insertions(+), 2409 deletions(-) delete mode 100644 capabilities2_fabric/CMakeLists.txt delete mode 100644 capabilities2_fabric/config/fabric.yaml delete mode 100644 capabilities2_fabric/docs/prompt_capability_runner_ex1.md delete mode 100644 capabilities2_fabric/docs/prompt_occupancy_runner_ex1.md delete mode 100644 capabilities2_fabric/docs/prompt_plan_runner_ex1.md delete mode 100644 capabilities2_fabric/docs/prompt_pose_runner_ex1.md delete mode 100644 capabilities2_fabric/docs/waypoint_runner_ex1.md delete mode 100644 capabilities2_fabric/docs/waypoint_runner_ex2.md delete mode 100644 capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric.hpp delete mode 100644 capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric_client.hpp delete mode 100644 capabilities2_fabric/include/capabilities2_fabric/utils/xml_parser.hpp delete mode 100644 capabilities2_fabric/launch/fabric.launch.py delete mode 100644 capabilities2_fabric/launch/fabric_composed.launch.py delete mode 100644 capabilities2_fabric/plans/default.xml delete mode 100644 capabilities2_fabric/plans/navigation_1.xml delete mode 100644 capabilities2_fabric/plans/navigation_2.xml delete mode 100644 capabilities2_fabric/plans/prompt_1.xml delete mode 100644 capabilities2_fabric/plans/prompt_2.xml delete mode 100644 capabilities2_fabric/plans/prompt_3.xml delete mode 100644 capabilities2_fabric/plans/prompt_4.xml delete mode 100644 capabilities2_fabric/readme.md delete mode 100644 capabilities2_fabric/src/capabilities_fabric_client_component.cpp delete mode 100644 capabilities2_fabric/src/capabilities_fabric_client_node.cpp delete mode 100644 capabilities2_fabric/src/capabilities_fabric_component.cpp delete mode 100644 capabilities2_fabric/src/capabilities_fabric_node.cpp delete mode 100644 capabilities2_msgs/action/Plan.action delete mode 100644 capabilities2_msgs/srv/CancelFabricPlan.srv delete mode 100644 capabilities2_msgs/srv/CompleteFabric.srv delete mode 100644 capabilities2_msgs/srv/GetFabricStatus.srv delete mode 100644 capabilities2_msgs/srv/SetFabricPlan.srv rename capabilities2_runner_capabilities/include/capabilities2_runner_capabilities/{capability_get_runner.hpp => get_runner.hpp} (100%) rename {capabilities2_fabric => capabilities2_runner_fabric}/.clang-format (99%) create mode 100644 capabilities2_runner_fabric/CMakeLists.txt rename capabilities2_runner_capabilities/include/capabilities2_runner_capabilities/fabric_completion_runner.hpp => capabilities2_runner_fabric/include/capabilities2_runner_fabric/completion_runner.hpp (74%) rename capabilities2_runner_capabilities/include/capabilities2_runner_capabilities/fabric_set_plan_runner.hpp => capabilities2_runner_fabric/include/capabilities2_runner_fabric/set_plan_runner.hpp (76%) rename {capabilities2_fabric => capabilities2_runner_fabric}/package.xml (75%) create mode 100644 capabilities2_runner_fabric/plugins.xml create mode 100644 capabilities2_runner_fabric/src/fabric_runners.cpp diff --git a/capabilities2_fabric/CMakeLists.txt b/capabilities2_fabric/CMakeLists.txt deleted file mode 100644 index 5e3a2d2..0000000 --- a/capabilities2_fabric/CMakeLists.txt +++ /dev/null @@ -1,159 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(capabilities2_fabric) - -# Default to C++17 -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) -endif() - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -# Find dependencies -find_package(ament_cmake REQUIRED) -find_package(rclcpp REQUIRED) -find_package(rclcpp_action REQUIRED) -find_package(rclcpp_components REQUIRED) -find_package(tinyxml2_vendor REQUIRED) -find_package(TinyXML2 REQUIRED) # provided by tinyxml2 upstream, or tinyxml2_vendor -find_package(bondcpp REQUIRED) -find_package(backward_ros REQUIRED) -find_package(capabilities2_msgs REQUIRED) -find_package(capabilities2_events REQUIRED) -find_package(capabilities2_utils REQUIRED) - -include_directories( - include -) - -############################################################################ -# capabilities2_fabric node executable -############################################################################ - -add_executable(${PROJECT_NAME} - src/capabilities_fabric_node.cpp -) - -ament_target_dependencies(${PROJECT_NAME} - rclcpp - rclcpp_action - bondcpp - capabilities2_msgs - capabilities2_events - capabilities2_utils - TinyXML2 -) - -install(TARGETS ${PROJECT_NAME} - DESTINATION lib/${PROJECT_NAME} -) - -############################################################################ -# capabilities2_fabric component library -############################################################################ - -add_library(${PROJECT_NAME}_comp SHARED - src/capabilities_fabric_component.cpp -) - -ament_target_dependencies(${PROJECT_NAME}_comp - rclcpp - rclcpp_action - rclcpp_components - bondcpp - capabilities2_msgs - capabilities2_events - capabilities2_utils - TinyXML2 -) - -rclcpp_components_register_node(${PROJECT_NAME}_comp - PLUGIN "capabilities2_fabric::CapabilitiesFabric" - EXECUTABLE capabilities2_fabric_component -) - -ament_export_targets(capabilities2_fabric_component) - -install(TARGETS ${PROJECT_NAME}_comp - EXPORT capabilities2_fabric_component - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION bin -) - -############################################################################ -# fabric_client node executable -############################################################################ - -add_executable(fabric_client - src/capabilities_fabric_client_node.cpp -) - -ament_target_dependencies(fabric_client - rclcpp - rclcpp_action - capabilities2_msgs - capabilities2_events - capabilities2_utils - TinyXML2 -) - -install(TARGETS fabric_client - DESTINATION lib/${PROJECT_NAME} -) - -############################################################################ -# fabric_client component library -############################################################################ - -add_library(fabric_client_comp SHARED - src/capabilities_fabric_client_component.cpp -) - -ament_target_dependencies(fabric_client_comp - rclcpp - rclcpp_action - rclcpp_components - bondcpp - capabilities2_msgs - capabilities2_events - capabilities2_utils - TinyXML2 -) - -rclcpp_components_register_node(fabric_client_comp - PLUGIN "capabilities2_fabric::CapabilitiesFabricClient" - EXECUTABLE capabilities2_fabric_client_component -) - -ament_export_targets(capabilities2_fabric_client_component) - -install(TARGETS fabric_client_comp - EXPORT capabilities2_fabric_client_component - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION bin -) - -############################################################################ -# Miscellaneous installations -############################################################################ - -install(DIRECTORY include/ - DESTINATION include -) - -install(DIRECTORY launch - DESTINATION share/${PROJECT_NAME} -) - -install(DIRECTORY config - DESTINATION share/${PROJECT_NAME} -) - -install(DIRECTORY plans - DESTINATION share/${PROJECT_NAME} -) - -ament_package() diff --git a/capabilities2_fabric/config/fabric.yaml b/capabilities2_fabric/config/fabric.yaml deleted file mode 100644 index 8a94606..0000000 --- a/capabilities2_fabric/config/fabric.yaml +++ /dev/null @@ -1,9 +0,0 @@ -fabric_client: - ros__parameters: - plan_file_path: "install/capabilities2_fabric/share/capabilities2_fabric/plans/prompt_4.xml" # PromptPlanRunner Example - # plan_file_path: "install/capabilities2_fabric/share/capabilities2_fabric/plans/prompt_3.xml" # PromptPoseRunner Example - # plan_file_path: "install/capabilities2_fabric/share/capabilities2_fabric/plans/prompt_2.xml" # PromptOccupancyRunner Example - # plan_file_path: "install/capabilities2_fabric/share/capabilities2_fabric/plans/prompt_1.xml" # PromptCapabilityRunner Example - # plan_file_path: "install/capabilities2_fabric/share/capabilities2_fabric/plans/navigation_2.xml" # WaypointRunner Example 2 - # plan_file_path: "install/capabilities2_fabric/share/capabilities2_fabric/plans/navigation_1.xml" # WaypointRunner Example 1 - # plan_file_path: "install/capabilities2_fabric/share/capabilities2_fabric/plans/default.xml" diff --git a/capabilities2_fabric/docs/prompt_capability_runner_ex1.md b/capabilities2_fabric/docs/prompt_capability_runner_ex1.md deleted file mode 100644 index 0028ccb..0000000 --- a/capabilities2_fabric/docs/prompt_capability_runner_ex1.md +++ /dev/null @@ -1,45 +0,0 @@ -## PromptCapabilityRunner Example - -### Dependencies - -This example uses prompt tools stack. Follow instructions from [Propmt Tools Dependency Installation](../../docs/prompt_tools_setup.md) to setup Prompt tools stack. - -### Plan selection - -Uncomment the line related to `prompt_1.xml` in the `config/fabric,yaml` file - -### Build the package to apply changes - -In the workspace root run, - -```bash -colcon build -``` - -### Start the Prompt Tools stack - -```bash -source install/setup.bash -ros2 launch prompt_bridge prompt_bridge.launch.py -``` - -### Start the Capabilities2 Server - -```bash -source install/setup.bash -ros2 launch capabilities2_server server.launch.py -``` - -### Start the Capabilities2 Fabric - -```bash -source install/setup.bash -ros2 launch capabilities2_fabric fabric.launch.py -``` - -### Start the Capabilities2 Event Listener (Optional for Debugging) - -```bash -source install/setup.bash -ros2 launch capabilities2_events listener.launch.py -``` \ No newline at end of file diff --git a/capabilities2_fabric/docs/prompt_occupancy_runner_ex1.md b/capabilities2_fabric/docs/prompt_occupancy_runner_ex1.md deleted file mode 100644 index 52c6ea3..0000000 --- a/capabilities2_fabric/docs/prompt_occupancy_runner_ex1.md +++ /dev/null @@ -1,59 +0,0 @@ -## PromptOccupancyRunner Example - -### Dependencies - -This example uses prompt tools stack, nav2 stack and turtlebot3. Follow instructions from [Nav2 Dependency Installation](../../docs/nav2_setup.md) to setup nav stack and [Propmt Tools Dependency Installation](../../docs/prompt_tools_setup.md) to setup nav stack. - -### Plan selection - -Uncomment the line related to `prompt_2.xml` in the `config/fabric,yaml` file - -### Build the package to apply changes - -In the workspace root run, - -```bash -colcon build -``` - -### Start the turtlebot simulation - -```bash -export TURTLEBOT3_MODEL=waffle -ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py -``` - -### Start the Navigation2 stack - -```bash -source install/setup.bash -ros2 launch nav_stack system.launch.py -``` - -### Start the Prompt Tools stack - -```bash -source install/setup.bash -ros2 launch prompt_bridge prompt_bridge.launch.py -``` - -### Start the Capabilities2 Server - -```bash -source install/setup.bash -ros2 launch capabilities2_server server.launch.py -``` - -### Start the Capabilities2 Fabric - -```bash -source install/setup.bash -ros2 launch capabilities2_fabric fabric.launch.py -``` - -### Start the Capabilities2 Event Listener (Optional for Debugging) - -```bash -source install/setup.bash -ros2 launch capabilities2_events listener.launch.py -``` \ No newline at end of file diff --git a/capabilities2_fabric/docs/prompt_plan_runner_ex1.md b/capabilities2_fabric/docs/prompt_plan_runner_ex1.md deleted file mode 100644 index bdd8b30..0000000 --- a/capabilities2_fabric/docs/prompt_plan_runner_ex1.md +++ /dev/null @@ -1,59 +0,0 @@ -## PromptPlanRunner Example - -### Dependencies - -This example uses prompt tools stack, nav2 stack and turtlebot3. Follow instructions from [Nav2 Dependency Installation](../../docs/nav2_setup.md) to setup nav stack and [Propmt Tools Dependency Installation](../../docs/prompt_tools_setup.md) to setup nav stack. - -### Plan selection - -Uncomment the line related to `prompt_4.xml` in the `config/fabric,yaml` file - -### Build the package to apply changes - -In the workspace root run, - -```bash -colcon build -``` - -### Start the turtlebot simulation - -```bash -export TURTLEBOT3_MODEL=waffle -ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py -``` - -### Start the Navigation2 stack - -```bash -source install/setup.bash -ros2 launch nav_stack system.launch.py -``` - -### Start the Prompt Tools stack - -```bash -source install/setup.bash -ros2 launch prompt_bridge prompt_bridge.launch.py -``` - -### Start the Capabilities2 Server - -```bash -source install/setup.bash -ros2 launch capabilities2_server server.launch.py -``` - -### Start the Capabilities2 Fabric - -```bash -source install/setup.bash -ros2 launch capabilities2_fabric fabric.launch.py -``` - -### Start the Capabilities2 Event Listener (Optional for Debugging) - -```bash -source install/setup.bash -ros2 launch capabilities2_events listener.launch.py -``` \ No newline at end of file diff --git a/capabilities2_fabric/docs/prompt_pose_runner_ex1.md b/capabilities2_fabric/docs/prompt_pose_runner_ex1.md deleted file mode 100644 index d572598..0000000 --- a/capabilities2_fabric/docs/prompt_pose_runner_ex1.md +++ /dev/null @@ -1,59 +0,0 @@ -## PromptPoseRunner Example - -### Dependencies - -This example uses prompt tools stack, nav2 stack and turtlebot3. Follow instructions from [Nav2 Dependency Installation](../../docs/nav2_setup.md) to setup nav stack and [Propmt Tools Dependency Installation](../../docs/prompt_tools_setup.md) to setup nav stack. - -### Plan selection - -Uncomment the line related to `prompt_3.xml` in the `config/fabric,yaml` file - -### Build the package to apply changes - -In the workspace root run, - -```bash -colcon build -``` - -### Start the turtlebot simulation - -```bash -export TURTLEBOT3_MODEL=waffle -ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py -``` - -### Start the Navigation2 stack - -```bash -source install/setup.bash -ros2 launch nav_stack system.launch.py -``` - -### Start the Prompt Tools stack - -```bash -source install/setup.bash -ros2 launch prompt_bridge prompt_bridge.launch.py -``` - -### Start the Capabilities2 Server - -```bash -source install/setup.bash -ros2 launch capabilities2_server server.launch.py -``` - -### Start the Capabilities2 Fabric - -```bash -source install/setup.bash -ros2 launch capabilities2_fabric fabric.launch.py -``` - -### Start the Capabilities2 Event Listener (Optional for Debugging) - -```bash -source install/setup.bash -ros2 launch capabilities2_events listener.launch.py -``` \ No newline at end of file diff --git a/capabilities2_fabric/docs/waypoint_runner_ex1.md b/capabilities2_fabric/docs/waypoint_runner_ex1.md deleted file mode 100644 index ef0d785..0000000 --- a/capabilities2_fabric/docs/waypoint_runner_ex1.md +++ /dev/null @@ -1,52 +0,0 @@ -## WaypointRunner Example 1 - Single Goal - -### Dependencies - -This example uses nav2 stack and turtlebot3. Follow instructions from [Nav2 Dependency Installation](../../docs/nav2_setup.md) to setup nav stack. - -### Plan selection - -Uncomment the line related to `navigation_1.xml` in the `config/fabric,yaml` file - -### Build the package to apply changes - -In the workspace root run, - -```bash -colcon build -``` - -### Start the turtlebot simulation - -```bash -export TURTLEBOT3_MODEL=waffle -ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py -``` - -### Start the Navigation2 stack - -```bash -source install/setup.bash -ros2 launch nav_stack system.launch.py -``` - -### Start the Capabilities2 Server - -```bash -source install/setup.bash -ros2 launch capabilities2_server server.launch.py -``` - -### Start the Capabilities2 Fabric - -```bash -source install/setup.bash -ros2 launch capabilities2_fabric fabric.launch.py -``` - -### Start the Capabilities2 Event Listener (Optional for Debugging) - -```bash -source install/setup.bash -ros2 launch capabilities2_events listener.launch.py -``` \ No newline at end of file diff --git a/capabilities2_fabric/docs/waypoint_runner_ex2.md b/capabilities2_fabric/docs/waypoint_runner_ex2.md deleted file mode 100644 index b13df68..0000000 --- a/capabilities2_fabric/docs/waypoint_runner_ex2.md +++ /dev/null @@ -1,52 +0,0 @@ -## WaypointRunner Example 2 - Goal Sequence - -### Dependencies - -This example uses nav2 stack and turtlebot3. Follow instructions from [Nav2 Dependency Installation](../../docs/nav2_setup.md) to setup nav stack. - -### Plan selection - -Uncomment the line related to `navigation_2.xml` in the `config/fabric,yaml` file - -### Build the package to apply changes - -In the workspace root run, - -```bash -colcon build -``` - -### Start the turtlebot simulation - -```bash -export TURTLEBOT3_MODEL=waffle -ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py -``` - -### Start the Navigation2 stack - -```bash -source install/setup.bash -ros2 launch nav_stack system.launch.py -``` - -### Start the Capabilities2 Server - -```bash -source install/setup.bash -ros2 launch capabilities2_server server.launch.py -``` - -### Start the Capabilities2 Fabric - -```bash -source install/setup.bash -ros2 launch capabilities2_fabric fabric.launch.py -``` - -### Start the Capabilities2 Event Listener (Optional for Debugging) - -```bash -source install/setup.bash -ros2 launch capabilities2_events listener.launch.py -``` \ No newline at end of file diff --git a/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric.hpp b/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric.hpp deleted file mode 100644 index 6372066..0000000 --- a/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric.hpp +++ /dev/null @@ -1,874 +0,0 @@ -#include -#include -#include -#include -#include -#include - -#include -#include - -#include -#include - -#include - -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -/** - * @brief Capabilities Fabric - * - * Capabilities fabric node that provides a ROS client for the capabilities server. - * Able to receive a XML file that implements a plan via action server that it exposes - * - */ - -class CapabilitiesFabric : public rclcpp::Node -{ -public: - using Plan = capabilities2_msgs::action::Plan; - using GoalHandlePlan = rclcpp_action::ServerGoalHandle; - - using GetInterfaces = capabilities2_msgs::srv::GetInterfaces; - using GetSemanticInterfaces = capabilities2_msgs::srv::GetSemanticInterfaces; - using GetProviders = capabilities2_msgs::srv::GetProviders; - using EstablishBond = capabilities2_msgs::srv::EstablishBond; - using UseCapability = capabilities2_msgs::srv::UseCapability; - using FreeCapability = capabilities2_msgs::srv::FreeCapability; - using ConfigureCapability = capabilities2_msgs::srv::ConfigureCapability; - using TriggerCapability = capabilities2_msgs::srv::TriggerCapability; - - using GetInterfacesClient = rclcpp::Client; - using GetSemanticInterfacesClient = rclcpp::Client; - using GetProvidersClient = rclcpp::Client; - using EstablishBondClient = rclcpp::Client; - using UseCapabilityClient = rclcpp::Client; - using FreeCapabilityClient = rclcpp::Client; - using ConfigureCapabilityClient = rclcpp::Client; - using TriggerCapabilityClient = rclcpp::Client; - - CapabilitiesFabric(const rclcpp::NodeOptions& options = rclcpp::NodeOptions()) : Node("Capabilities2_Fabric", options) - { - try - { - // Only call setup if this object is already owned by a shared_ptr - if (shared_from_this()) - { - initialize(); - } - } - catch (const std::bad_weak_ptr&) - { - // Not yet safe — probably standalone without make_shared - } - } - - /** - * @brief Initializer function for Ccapabilities2 fabric. - * Configure the action server for the capabilieites fabric and configure server clients for the capability runners from the - * capabilities2 server - * - */ - void initialize() - { - control_tag_list = xml_parser::get_control_list(); - - event_ = std::make_shared(shared_from_this(), "fabric", "/events"); - - this->planner_server_ = rclcpp_action::create_server( - this, "/capabilities_fabric", std::bind(&CapabilitiesFabric::handle_goal, this, std::placeholders::_1, std::placeholders::_2), - std::bind(&CapabilitiesFabric::handle_cancel, this, std::placeholders::_1), - std::bind(&CapabilitiesFabric::handle_accepted, this, std::placeholders::_1)); - - get_interfaces_client_ = this->create_client("/capabilities/get_interfaces"); - get_sem_interf_client_ = this->create_client("/capabilities/get_semantic_interfaces"); - get_providers_client_ = this->create_client("/capabilities/get_providers"); - establish_bond_client_ = this->create_client("/capabilities/establish_bond"); - use_capability_client_ = this->create_client("/capabilities/use_capability"); - free_capability_client_ = this->create_client("/capabilities/free_capability"); - trig_capability_client_ = this->create_client("/capabilities/trigger_capability"); - conf_capability_client_ = this->create_client("/capabilities/configure_capability"); - - // Wait for services to become available - check_service(!get_interfaces_client_->wait_for_service(std::chrono::seconds(1)), "/capabilities/get_interfaces"); - check_service(!get_sem_interf_client_->wait_for_service(std::chrono::seconds(1)), "/capabilities/get_semantic_interfaces"); - check_service(!get_providers_client_->wait_for_service(std::chrono::seconds(1)), "/capabilities/get_providers"); - check_service(!establish_bond_client_->wait_for_service(std::chrono::seconds(1)), "/capabilities/establish_bond"); - check_service(!use_capability_client_->wait_for_service(std::chrono::seconds(1)), "/capabilities/use_capability"); - check_service(!free_capability_client_->wait_for_service(std::chrono::seconds(1)), "/capabilities/free_capability"); - check_service(!trig_capability_client_->wait_for_service(std::chrono::seconds(1)), "/capabilities/trigger_capability"); - check_service(!conf_capability_client_->wait_for_service(std::chrono::seconds(1)), "/capabilities/configure_capability"); - - result_msg = std::make_shared(); - } - -private: - /** - * @brief Handle the goal request that comes in from client. returns whether goal is accepted or rejected - * - * - * @param uuid uuid of the goal - * @param goal pointer to the action goal message - * @return rclcpp_action::GoalResponse - */ - rclcpp_action::GoalResponse handle_goal(const rclcpp_action::GoalUUID& uuid, std::shared_ptr goal) - { - event_->info("Received the goal request with the plan"); - - (void)uuid; - - event_->info("Following plan was received :\n\n " + goal->plan); - - // try to parse the std::string plan from capabilities_msgs/Plan to the to a XMLDocument file - tinyxml2::XMLError xml_status = document.Parse(goal->plan.c_str()); - - // check if the file parsing failed - if (xml_status != tinyxml2::XMLError::XML_SUCCESS) - { - event_->error("Parsing the plan from goal message failed"); - return rclcpp_action::GoalResponse::REJECT; - } - - event_->info("Plan parsed and accepted"); - return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; - } - - /** - * @brief Handle the goal cancel request that comes in from client. - * - * @param goal_handle pointer to the action goal handle - * @return rclcpp_action::GoalResponse - */ - rclcpp_action::CancelResponse handle_cancel(const std::shared_ptr goal_handle) - { - event_->info("Received the request to cancel the plan"); - (void)goal_handle; - - for (auto& [bond_id, bond_client] : bond_client_cache_) - { - bond_client->stop(); - } - - return rclcpp_action::CancelResponse::ACCEPT; - } - - /** - * @brief Handle the goal accept event originating from handle_goal. - * - * @param goal_handle pointer to the action goal handle - */ - void handle_accepted(const std::shared_ptr goal_handle) - { - goal_handle_ = goal_handle; - event_->info("Goal handle accepted"); - - execution(); - } - - /** - * @brief Start the execution of the capabilities2 fabric - */ - void execution() - { - event_->info("A new execution started"); - - xml_parser::add_closing_event(document); - xml_parser::convert_to_string(document, modified_plan); - - event_->info("Plan after adding closing event :\n\n " + modified_plan); - - interface_list.clear(); - providers_list.clear(); - rejected_list.clear(); - connection_map.clear(); - - expected_providers_ = 0; - completed_providers_ = 0; - - expected_interfaces_ = 0; - completed_interfaces_ = 0; - - expected_capabilities_ = 0; - completed_capabilities_ = 0; - freed_capabilities_ = 0; - - expected_configurations_ = 0; - completed_configurations_ = 0; - - getInterfaces(); - } - - /** - * @brief Get Interfaces available in the capabilities2 server via relavant service - */ - void getInterfaces() - { - event_->info("Requesting Interface information"); - - auto request_interface = std::make_shared(); - - // request data from the server - auto result_future = get_interfaces_client_->async_send_request(request_interface, [this](GetInterfacesClient::SharedFuture future) { - auto result = std::make_shared(); - - if (!future.valid()) - { - result_msg->success = false; - result_msg->message = "Failed to get Interface information. Server execution cancelled"; - event_->error(result_msg->message); - goal_handle_->abort(result_msg); - return; - } - - auto response = future.get(); - expected_interfaces_ = response->interfaces.size(); - - event_->info("Received Interfaces. Requsting " + std::to_string(expected_interfaces_) + " semantic interface information"); - - // Request each interface recursively for Semantic interfaces - getSemanticInterfaces(response->interfaces); - }); - } - - /** - * @brief Get the Semantic Interfaces from the capabilities2 server via related service client - * - * @param interfaces std::vector of interfaces for which the semantic interfaces will be requested - */ - void getSemanticInterfaces(const std::vector& interfaces) - { - std::string requested_interface = interfaces[completed_interfaces_]; - - event_->info("Requesting semantic interfaces for " + requested_interface); - - auto request_semantic = std::make_shared(); - request_semantic->interface = requested_interface; - - // request semantic interface from the server - auto result_semantic_future = get_sem_interf_client_->async_send_request( - request_semantic, [this, interfaces, requested_interface](GetSemanticInterfacesClient::SharedFuture future) { - if (!future.valid()) - { - result_msg->success = false; - result_msg->message = "Failed to get Semantic Interface information. Server execution cancelled"; - event_->error(result_msg->message); - goal_handle_->abort(result_msg); - return; - } - - completed_interfaces_++; - auto response = future.get(); - - // if semantic interfaces are availble for a given interface, add the semantic interface - if (response->semantic_interfaces.size() > 0) - { - for (const auto& semantic_interface : response->semantic_interfaces) - { - interface_list.push_back(semantic_interface); - is_semantic_list.push_back(true); - - event_->info(std::to_string(completed_interfaces_) + "/" + std::to_string(expected_interfaces_) + " : Received " + semantic_interface + - " for " + requested_interface + ". So added " + semantic_interface); - } - } - // if no semantic interfaces are availble for a given interface, add the interface instead - else - { - interface_list.push_back(requested_interface); - is_semantic_list.push_back(false); - - event_->info(std::to_string(completed_interfaces_) + "/" + std::to_string(expected_interfaces_) + " : Received none for " + - requested_interface + ". So added " + requested_interface); - } - - if (completed_interfaces_ != expected_interfaces_) - { - // Request next interface recursively for Semantic interfaces - getSemanticInterfaces(interfaces); - } - else - { - event_->info("Received all requested Interface information"); - - expected_providers_ = interface_list.size(); - - event_->info("Requsting Provider information for " + std::to_string(expected_providers_) + " providers"); - - // request providers from the interfaces in the interfaces_list - getProvider(interface_list, is_semantic_list); - } - }); - } - - /** - * @brief Get the Provider information for the related interfaces - * - * @param interfaces std::vector of interfaces - * @param is_semantic std::vector of masks about interfaces with true value for semantic interfaces - */ - void getProvider(const std::vector& interfaces, const std::vector& is_semantic) - { - std::string requested_interface = interfaces[completed_providers_]; - bool semantic_flag = is_semantic[completed_providers_]; - - event_->info("Requesting provider for " + requested_interface); - - auto request_providers = std::make_shared(); - - // request providers of the semantic interface - request_providers->interface = requested_interface; - request_providers->include_semantic = semantic_flag; - - auto result_providers_future = get_providers_client_->async_send_request(request_providers, [this, is_semantic, requested_interface, interfaces]( - GetProvidersClient::SharedFuture future) { - if (!future.valid()) - { - result_msg->success = false; - result_msg->message = "Failed to retrieve providers for interface: " + requested_interface; - event_->error(result_msg->message); - goal_handle_->abort(result_msg); - return; - } - - completed_providers_++; - auto response = future.get(); - - if (response->default_provider != "") - { - // add defualt provider to the list - providers_list.push_back(response->default_provider); - - event_->info(std::to_string(completed_providers_) + "/" + std::to_string(expected_providers_) + " : Received " + response->default_provider + - " for " + requested_interface + ". So added " + response->default_provider); - } - - // add additional providers to the list if available - if (response->providers.size() > 0) - { - for (const auto& provider : response->providers) - { - providers_list.push_back(provider); - - event_->info(std::to_string(completed_providers_) + "/" + std::to_string(expected_providers_) + " : Received and added " + provider + - " for " + requested_interface); - } - } - else - { - event_->info(std::to_string(completed_providers_) + "/" + std::to_string(expected_providers_) + " : No providers for " + requested_interface); - } - - // Check if all expected calls are completed before calling verify_plan - if (completed_providers_ != expected_providers_) - { - // request providers for the next interface in the interfaces_list - getProvider(interfaces, is_semantic); - } - else - { - event_->info("All requested interface, semantic interface and provider data recieved"); - - verify_and_continue(); - } - }); - } - - /** - * @brief Verify the plan before continuing the execution using xml parsing and collected interface, semantic interface - * and provider information - * - */ - void verify_and_continue() - { - event_->info("Verifying the plan"); - - bool verification_success = true; - - auto result = std::make_shared(); - - // extract the components within the 'plan' tags - bool extraction_success = false; - tinyxml2::XMLElement* plan = xml_parser::get_plan(document, extraction_success); - - if (!extraction_success) - { - result_msg->success = false; - result_msg->message = "Execution plan is not compatible. Please recheck and update"; - event_->error(result_msg->message); - goal_handle_->abort(result_msg); - verification_success = false; - } - - event_->info("Plan extraction complete"); - - // verify whether the plan is valid by checking the tags - std::string error_message; - - if (!xml_parser::check_tags(plan, interface_list, providers_list, control_tag_list, rejected_list, error_message)) - { - result_msg->success = false; - result_msg->message = "Execution plan is faulty. Please recheck and update"; - event_->error(result_msg->message); - goal_handle_->abort(result_msg); - verification_success = false; - } - - event_->info("Checking tags successful"); - - // verify the plan - if (!verification_success) - { - event_->info("Plan verification failed"); - - if (rejected_list.size() > 0) - { - // TODO: improve with error codes - auto result = std::make_shared(); - result->success = false; - result->message = "Plan verification failed. There are mismatched events"; - - for (const auto& rejected_element : rejected_list) - { - result->failed_elements.push_back(rejected_element); - } - - goal_handle_->abort(result); - event_->info(result->message); - } - else - { - // TODO: improve with error codes - result_msg->success = false; - result_msg->message = "Plan verification failed. Server Execution Cancelled."; - event_->error(result_msg->message); - goal_handle_->abort(result_msg); - return; - } - - event_->error("Server Execution Cancelled"); - } - - event_->info("Plan verification successful. Proceeding with connections extraction"); - - // Extract the connections from the plan - xml_parser::extract_connections(plan, connection_map); - - event_->info("Connection extraction successful"); - - // estasblish the bond with the server - request_bond(); - } - - /** - * @brief Request the bond from the capabilities2 server - * - */ - void request_bond() - { - event_->info("Requesting bond id"); - - // create bond establishing server request - auto request_bond = std::make_shared(); - - // send the request - auto result_future = establish_bond_client_->async_send_request(request_bond, [this](EstablishBondClient::SharedFuture future) { - if (!future.valid()) - { - result_msg->success = false; - result_msg->message = "Failed to retrieve the bond id. Server execution cancelled"; - event_->error(result_msg->message); - goal_handle_->abort(result_msg); - return; - } - - auto response = future.get(); - bond_id_ = response->bond_id; - event_->info("Received the bond id : " + bond_id_); - - establish_bond(); - }); - } - - /** - * @brief establish the bond with capabilities2 server - * - */ - void establish_bond() - { - bond_client_cache_[bond_id_] = std::make_unique(shared_from_this(), bond_id_); - bond_client_cache_[bond_id_]->start(); - - event_->info("Bond sucessfully established with bond id : " + bond_id_); - - if (bond_client_cache_.size() > 1) - { - for (auto& [old_bond_id, bond_client] : bond_client_cache_) - { - if (old_bond_id != bond_id_) - { - bond_client->stop(); - event_->info("Stopping and removing old bond with id : " + old_bond_id); - } - } - } - - expected_capabilities_ = connection_map.size(); - - event_->info("Requsting start of " + std::to_string(expected_capabilities_) + " capabilities"); - - use_capability(connection_map); - } - - /** - * @brief Request use of capability from capabilities2 server - * - * @param capabilities capability list to be started - * @param provider provider of the capability - */ - void use_capability(std::map& capabilities) - { - std::string capability = capabilities[completed_capabilities_].source.runner; - std::string provider = capabilities[completed_capabilities_].source.provider; - - auto request_use = std::make_shared(); - request_use->capability = capability; - request_use->preferred_provider = provider; - request_use->bond_id = bond_id_; - - event_->info("Starting capability of Runner " + std::to_string(completed_capabilities_) + " : " + - capabilities[completed_capabilities_].source.runner); - - // send the request - auto result_future = - use_capability_client_->async_send_request(request_use, [this, capability, provider](UseCapabilityClient::SharedFuture future) { - if (!future.valid()) - { - result_msg->success = false; - result_msg->message = "Failed to Use capability " + capability + " from " + provider + ". Server Execution Cancelled"; - event_->error(result_msg->message); - goal_handle_->abort(result_msg); - - // release all capabilities that were used since not all started successfully - free_capability_all(connection_map); - - for (auto& [bond_id, bond_client] : bond_client_cache_) - { - bond_client->stop(); - } - return; - } - - completed_capabilities_++; - - auto response = future.get(); - - event_->info(std::to_string(completed_capabilities_) + "/" + std::to_string(expected_capabilities_) + " : start succeessful"); - - // Check if all expected calls are completed before calling verify_plan - if (completed_capabilities_ == expected_capabilities_) - { - event_->info("All requested capabilities have been started. Configuring the capabilities with events"); - - expected_configurations_ = connection_map.size(); - - event_->info("Requsting capability configuration for " + std::to_string(expected_configurations_) + " capabilities"); - - configure_capabilities(connection_map); - } - else - { - use_capability(connection_map); - } - }); - } - - /** - * @brief Free all started capabilities in the capabilities map - * - * @param capabilities map of capabilities to be freed - */ - void free_capability_all(std::map& capabilities) - { - std::string capability = capabilities[freed_capabilities_].source.runner; - - auto request_free = std::make_shared(); - request_free->capability = capability; - request_free->bond_id = bond_id_; - - // send the request - auto result_future = free_capability_client_->async_send_request(request_free, [this, capability](FreeCapabilityClient::SharedFuture future) { - if (!future.valid()) - { - result_msg->success = false; - result_msg->message = "Failed to free capability " + capability; - event_->error(result_msg->message); - goal_handle_->abort(result_msg); - return; - } - - auto response = future.get(); - event_->info("Successfully freed capability " + capability); - - freed_capabilities_++; - - // Check if all expected calls are completed before calling verify_plan - if (freed_capabilities_ == completed_capabilities_) - { - event_->info("All started capabilities have been freed."); - } - else - { - free_capability_all(connection_map); - } - }); - } - - /** - * @brief Request use of capability from capabilities2 server - */ - void configure_capabilities(std::map& capabilities) - { - auto request_configure = std::make_shared(); - - event_->info("Configuring capability of Runner " + std::to_string(completed_configurations_) + " named " + - capabilities[completed_configurations_].source.runner); - - if (xml_parser::convert_to_string(capabilities[completed_configurations_].source.parameters, request_configure->source.parameters)) - { - request_configure->source.capability = capabilities[completed_configurations_].source.runner; - request_configure->source.provider = capabilities[completed_configurations_].source.provider; - } - else - { - request_configure->source.capability = ""; - request_configure->source.provider = ""; - } - - if (xml_parser::convert_to_string(capabilities[completed_configurations_].target_on_start.parameters, - request_configure->target_on_start.parameters)) - { - request_configure->target_on_start.capability = capabilities[completed_configurations_].target_on_start.runner; - request_configure->target_on_start.provider = capabilities[completed_configurations_].target_on_start.provider; - } - else - { - request_configure->target_on_start.capability = ""; - request_configure->target_on_start.provider = ""; - } - - if (xml_parser::convert_to_string(capabilities[completed_configurations_].target_on_stop.parameters, - request_configure->target_on_stop.parameters)) - { - request_configure->target_on_stop.capability = capabilities[completed_configurations_].target_on_stop.runner; - request_configure->target_on_stop.provider = capabilities[completed_configurations_].target_on_stop.provider; - } - else - { - request_configure->target_on_stop.capability = ""; - request_configure->target_on_stop.provider = ""; - } - - if (xml_parser::convert_to_string(capabilities[completed_configurations_].target_on_success.parameters, - request_configure->target_on_success.parameters)) - { - request_configure->target_on_success.capability = capabilities[completed_configurations_].target_on_success.runner; - request_configure->target_on_success.provider = capabilities[completed_configurations_].target_on_success.provider; - } - else - { - request_configure->target_on_success.capability = ""; - request_configure->target_on_success.provider = ""; - } - - if (xml_parser::convert_to_string(capabilities[completed_configurations_].target_on_failure.parameters, - request_configure->target_on_failure.parameters)) - { - request_configure->target_on_failure.capability = capabilities[completed_configurations_].target_on_failure.runner; - request_configure->target_on_failure.provider = capabilities[completed_configurations_].target_on_failure.provider; - } - else - { - request_configure->target_on_failure.capability = ""; - request_configure->target_on_failure.provider = ""; - } - - std::string source_capability = capabilities[completed_configurations_].source.runner; - - // send the request - auto result_future = - conf_capability_client_->async_send_request(request_configure, [this, source_capability](ConfigureCapabilityClient::SharedFuture future) { - if (!future.valid()) - { - result_msg->success = false; - result_msg->message = "Failed to configure capability :" + source_capability + ". Server execution cancelled"; - event_->error(result_msg->message); - goal_handle_->abort(result_msg); - return; - } - - completed_configurations_++; - - auto response = future.get(); - - event_->info(std::to_string(completed_configurations_) + "/" + std::to_string(expected_configurations_) + - " : Successfully configured capability : " + source_capability); - - // Check if all expected calls are completed before calling verify_plan - if (completed_configurations_ == expected_configurations_) - { - event_->info("All requested capabilities have been configured. Triggering the first capability"); - - trigger_first_node(); - } - else - { - configure_capabilities(connection_map); - } - }); - } - - /** - * @brief Trigger the first node - */ - void trigger_first_node() - { - auto request_trigger = std::make_shared(); - - std::string parameter_string; - xml_parser::convert_to_string(connection_map[0].source.parameters, parameter_string); - request_trigger->capability = connection_map[0].source.runner; - request_trigger->parameters = parameter_string; - - // send the request - auto result_future = trig_capability_client_->async_send_request(request_trigger, [this](TriggerCapabilityClient::SharedFuture future) { - if (!future.valid()) - { - result_msg->success = false; - result_msg->message = "Failed to trigger capability " + connection_map[0].source.runner; - event_->error(result_msg->message); - goal_handle_->abort(result_msg); - return; - } - - auto response = future.get(); - event_->info("Successfully triggered capability " + connection_map[0].source.runner); - - result_msg->success = true; - result_msg->message = "Successfully completed capabilities2 fabric"; - event_->info(result_msg->message); - goal_handle_->succeed(result_msg); - }); - } - - void check_service(bool wait_for_logic, const std::string& service_name) - { - while (wait_for_logic) - { - event_->error(service_name + " not available"); - rclcpp::shutdown(); - return; - } - event_->info(service_name + " connected"); - } - -private: - /** File Path link */ - std::string plan_file_path; - - /** Modified plan with closing capabilities */ - std::string modified_plan; - - /** flag to select loading from file or accepting via action server */ - bool read_file; - - int expected_interfaces_; - int completed_interfaces_; - - int expected_providers_; - int completed_providers_; - - int expected_capabilities_; - int completed_capabilities_; - int freed_capabilities_; - - int expected_configurations_; - int completed_configurations_; - - /** Bond id */ - std::string bond_id_; - - /** Manages bond between capabilities server and this client */ - std::map> bond_client_cache_; - - /** XML Document */ - tinyxml2::XMLDocument document; - - /** vector of connections */ - std::map connection_map; - - /** Interface List */ - std::vector is_semantic_list; - - /** Interface List */ - std::vector interface_list; - - /** Providers List */ - std::vector providers_list; - - /** Control flow List */ - std::vector control_tag_list; - - /** Invalid events list */ - std::vector rejected_list; - - /** Result message for plan action server*/ - std::shared_ptr result_msg; - - /** action server that exposes executor*/ - std::shared_ptr> planner_server_; - - /** action server goal handle*/ - std::shared_ptr goal_handle_; - - /** Get interfaces from capabilities server */ - GetInterfacesClient::SharedPtr get_interfaces_client_; - - /** Get semantic interfaces from capabilities server */ - GetSemanticInterfacesClient::SharedPtr get_sem_interf_client_; - - /** Get providers from capabilities server */ - GetProvidersClient::SharedPtr get_providers_client_; - - /** establish bond */ - EstablishBondClient::SharedPtr establish_bond_client_; - - /** use an selected capability */ - UseCapabilityClient::SharedPtr use_capability_client_; - - /** free an selected capability */ - FreeCapabilityClient::SharedPtr free_capability_client_; - - /** configure an selected capability */ - ConfigureCapabilityClient::SharedPtr conf_capability_client_; - - /** trigger an selected capability */ - TriggerCapabilityClient::SharedPtr trig_capability_client_; - - /** Event client for publishing events */ - std::shared_ptr event_; - - /** capabilities2 server and fabric synchronization tools */ - // std::mutex mutex_; - // std::condition_variable cv_; - // bool fabric_completed_; - // std::unique_lock lock_; -}; \ No newline at end of file diff --git a/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric_client.hpp b/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric_client.hpp deleted file mode 100644 index 1434d0a..0000000 --- a/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric_client.hpp +++ /dev/null @@ -1,359 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include - -#include - -#include -#include -#include -#include -#include - -#include - -/** - * @brief Capabilities Executor File Parser - * - * Capabilities Executor File Parser node that provides a ROS client for the capabilities executor. - * Will read an XML file that implements a plan and send it to the server - */ - -class CapabilitiesFabricClient : public rclcpp::Node -{ - enum Status - { - IDLE, - RUNNING, - CANCELED, - ABORTED, - FAILED, - LAUNCHED, - COMPLETED - }; - -public: - using Plan = capabilities2_msgs::action::Plan; - using GoalHandlePlan = rclcpp_action::ClientGoalHandle; - - using GetFabricStatus = capabilities2_msgs::srv::GetFabricStatus; - using SetFabricPlan = capabilities2_msgs::srv::SetFabricPlan; - using CancelFabricPlan = capabilities2_msgs::srv::CancelFabricPlan; - using CompleteFabric = capabilities2_msgs::srv::CompleteFabric; - - CapabilitiesFabricClient(const rclcpp::NodeOptions& options = rclcpp::NodeOptions()) : Node("Capabilities2_Fabric_Client", options) - { - try - { - // Only call setup if this object is already owned by a shared_ptr - if (shared_from_this()) - { - initialize(); - } - } - catch (const std::bad_weak_ptr&) - { - // Not yet safe — probably standalone without make_shared - } - } - - /** - * @brief Initializer function - * - */ - void initialize() - { - declare_parameter("plan_file_path", "install/capabilities2_fabric/share/capabilities2_fabric/plans/default.xml"); - plan_file_path = get_parameter("plan_file_path").as_string(); - - fabric_state = Status::IDLE; - - event_ = std::make_shared(shared_from_this(), "client", "/events"); - - status_server_ = - this->create_service("/capabilities_fabric/get_status", std::bind(&CapabilitiesFabricClient::getStatusCallback, this, - std::placeholders::_1, std::placeholders::_2)); - - plan_server_ = this->create_service( - "/capabilities_fabric/set_plan", std::bind(&CapabilitiesFabricClient::setPlanCallback, this, std::placeholders::_1, std::placeholders::_2)); - - cancel_server_ = - this->create_service("/capabilities_fabric/cancel_plan", std::bind(&CapabilitiesFabricClient::cancelPlanCallback, this, - std::placeholders::_1, std::placeholders::_2)); - - completion_server_ = - this->create_service("/capabilities_fabric/set_completion", std::bind(&CapabilitiesFabricClient::setCompleteCallback, this, - std::placeholders::_1, std::placeholders::_2)); - - // Create the action client for capabilities_fabric after the node is fully constructed - this->planner_client_ = rclcpp_action::create_client(shared_from_this(), "/capabilities_fabric"); - - if (!this->planner_client_->wait_for_action_server(std::chrono::seconds(5))) - { - event_->error("Action server not available after waiting"); - rclcpp::shutdown(); - return; - } - - event_->info("Sucessfully connected to the capabilities_fabric action server"); - - // try to load the file - tinyxml2::XMLError xml_status = document.LoadFile(plan_file_path.c_str()); - - // check if the file loading failed - if (xml_status != tinyxml2::XMLError::XML_SUCCESS) - { - event_->error("Error loading plan: " + plan_file_path + ", Error: " + document.ErrorName()); - rclcpp::shutdown(); - } - - event_->info("Plan loaded from : " + plan_file_path); - - std::string plan; - xml_parser::convert_to_string(document, plan); - - plan_queue.push_back(plan); - - goal_send_thread = std::thread(&CapabilitiesFabricClient::manage_goal, this); - } - -private: - void manage_goal() - { - while (plan_queue.size() > 0) - { - event_->info("Fabric client thread starting"); - - std::unique_lock lock(mutex_); - completed_ = false; - - send_goal(); - - event_->info("Fabric plan sent. Waiting for acceptance."); - - // Conditional wait - cv_.wait(lock, [this] { return completed_; }); - event_->info("Fabric client thread closing"); - } - } - - void send_goal() - { - // Create Plan Goal message - auto goal_msg = Plan::Goal(); - - // load the latest plan from the queue - goal_msg.plan = plan_queue[0]; - plan_queue.pop_front(); - - event_->info("Sending goal to the capabilities_fabric action server"); - - // send goal options - auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); - - // goal response callback - send_goal_options.goal_response_callback = [this](const GoalHandlePlan::SharedPtr& goal_handle) { - if (!goal_handle) - { - event_->error("Goal was rejected by server"); - fabric_state = Status::FAILED; - } - else - { - event_->info("Goal accepted by server, waiting for completion"); - goal_handle_ = goal_handle; - fabric_state = Status::RUNNING; - } - }; - - // result callback - send_goal_options.result_callback = [this](const GoalHandlePlan::WrappedResult& result) { - switch (result.code) - { - case rclcpp_action::ResultCode::SUCCEEDED: - fabric_state = Status::LAUNCHED; - break; - case rclcpp_action::ResultCode::ABORTED: - event_->error("Goal was aborted"); - fabric_state = Status::ABORTED; - break; - case rclcpp_action::ResultCode::CANCELED: - event_->error("Goal was canceled"); - fabric_state = Status::CANCELED; - break; - default: - event_->error("Unknown result code"); - fabric_state = Status::FAILED; - break; - } - - if (result.result->success) - { - event_->info("Plan launched successfully"); - } - else - { - event_->error("Plan failed to launch"); - - if (result.result->failed_elements.size() > 0) - { - event_->error("Plan failed due to incompatible XMLElements in the plan"); - - for (const auto& failed_element : result.result->failed_elements) - event_->error_element(failed_element); - } - } - }; - - this->planner_client_->async_send_goal(goal_msg, send_goal_options); - } - - void cancelPlanCallback(const std::shared_ptr request, std::shared_ptr response) - { - if (fabric_state == Status::RUNNING) - { - event_->info("Plan canncelling requested"); - this->planner_client_->async_cancel_goal(goal_handle_); - } - - response->success = true; - } - - void setCompleteCallback(const std::shared_ptr request, std::shared_ptr response) - { - fabric_state = Status::COMPLETED; - event_->info("Plan completed successfully"); - completed_ = true; - cv_.notify_all(); - } - - void getStatusCallback(const std::shared_ptr request, std::shared_ptr response) - { - if (fabric_state == Status::IDLE) - { - response->status = GetFabricStatus::Response::FABRIC_IDLE; - } - else if (fabric_state == Status::RUNNING) - { - response->status = GetFabricStatus::Response::FABRIC_RUNNING; - } - else if (fabric_state == Status::CANCELED) - { - response->status = GetFabricStatus::Response::FABRIC_CANCELED; - } - else if (fabric_state == Status::ABORTED) - { - response->status = GetFabricStatus::Response::FABRIC_ABORTED; - } - else if (fabric_state == Status::FAILED) - { - response->status = GetFabricStatus::Response::FABRIC_FAILED; - } - else if (fabric_state == Status::LAUNCHED) - { - response->status = GetFabricStatus::Response::FABRIC_LAUNCHED; - } - else if (fabric_state == Status::COMPLETED) - { - response->status = GetFabricStatus::Response::FABRIC_COMPLETED; - } - else - { - response->status = GetFabricStatus::Response::UNKNOWN; - } - } - - void setPlanCallback(const std::shared_ptr request, std::shared_ptr response) - { - event_->info("Received the request with a plan"); - - // try to parse the std::string plan from capabilities_msgs/Plan to the to a XMLDocument file - tinyxml2::XMLError xml_status = documentChecking.Parse(request->plan.c_str()); - - // check if the file parsing failed - if (xml_status != tinyxml2::XMLError::XML_SUCCESS) - { - event_->info("Parsing the plan from service request message failed"); - response->success = false; - } - - event_->info("Plan parsed and valid"); - - plan_queue.push_back(request->plan); - - event_->info("Plan queued and waiting for execution"); - - if ((fabric_state == Status::RUNNING) or (fabric_state == Status::LAUNCHED)) - { - event_->info("Prior plan under exeution. Will defer the new plan"); - } - else - { - event_->info("Plan parsed and accepted"); - goal_send_thread = std::thread(&CapabilitiesFabricClient::manage_goal, this); - } - - response->success = true; - } - -private: - /** File Path link */ - std::string plan_file_path; - - /** Status message */ - std::string status; - - /** Vector of plans */ - std::deque plan_queue; - - /** XML Document */ - tinyxml2::XMLDocument document; - - /** XML Document */ - tinyxml2::XMLDocument documentChecking; - - /** Thread to manage sending goal */ - std::thread goal_send_thread; - - /** action client */ - rclcpp_action::Client::SharedPtr planner_client_; - - /** Goal handle for action client control */ - GoalHandlePlan::SharedPtr goal_handle_; - - /** server to get the status of the capabilities2 fabric */ - rclcpp::Service::SharedPtr status_server_; - - /** server to set a new plan to the capabilities2 fabric */ - rclcpp::Service::SharedPtr plan_server_; - - /** server to cancel the current plan in the capabilities2 fabric */ - rclcpp::Service::SharedPtr cancel_server_; - - /** server to get the status of the capabilities2 fabric */ - rclcpp::Service::SharedPtr completion_server_; - - /** Status of the fabric */ - Status fabric_state; - - /** Event client for publishing events */ - std::shared_ptr event_; - - /** mutex for threadpool synchronisation. */ - std::mutex mutex_; - - /** conditional variable for threadpool synchronisation */ - std::condition_variable cv_; - - /** flag for threadpool synchronisation. */ - bool completed_; -}; diff --git a/capabilities2_fabric/include/capabilities2_fabric/utils/xml_parser.hpp b/capabilities2_fabric/include/capabilities2_fabric/utils/xml_parser.hpp deleted file mode 100644 index d3b8e92..0000000 --- a/capabilities2_fabric/include/capabilities2_fabric/utils/xml_parser.hpp +++ /dev/null @@ -1,313 +0,0 @@ -#pragma once -#include -#include -#include -#include -#include -#include - -namespace xml_parser -{ -/** - * @brief extract elements related plan and return the first child element - * - * @param document XML document to extract plan from - * @param success boolean to indicate if the plan was found - * - * @return plan in the form of tinyxml2::XMLElement* - */ -tinyxml2::XMLElement* get_plan(tinyxml2::XMLDocument& document, bool& success) -{ - std::string plan_tag(document.FirstChildElement()->Name()); - - if (plan_tag == "Plan") - { - success = true; - return document.FirstChildElement("Plan")->FirstChildElement(); - } - else - { - success = false; - return nullptr; - } -} - -/** - * @brief search a string in a vector of strings - * - * @param list vector of strings to be searched - * @param value string to be searched in the vector - * - * @return `true` if value is found in list and `false` otherwise - */ -bool search(std::vector list, std::string value) -{ - return (std::find(list.begin(), list.end(), value) != list.end()); -} - -/** - * @brief convert XMLElement to std::string - * - * @param element XMLElement element to be converted - * @param paramters parameter to hold std::string - * - * @return `true` if element is not nullptr and conversion successful, `false` if element is nullptr - */ -bool convert_to_string(tinyxml2::XMLElement* element, std::string& parameters) -{ - if (element) - { - tinyxml2::XMLPrinter printer; - element->Accept(&printer); - parameters = printer.CStr(); - return true; - } - else - { - parameters = ""; - return false; - } -} - -/** - * @brief convert XMLDocument to std::string - * - * @param document element to be converted - * - * @return std::string converted document - */ -void convert_to_string(tinyxml2::XMLDocument& document_xml, std::string& document_string) -{ - tinyxml2::XMLPrinter printer; - document_xml.Print(&printer); - document_string = printer.CStr(); -} - -void add_closing_event(tinyxml2::XMLDocument& document) -{ - // Get the root element - tinyxml2::XMLElement* plan = document.FirstChildElement("Plan"); - - // Get the existing element inside - tinyxml2::XMLElement* innerControl = plan->FirstChildElement("Control"); - - // Create the outer element - tinyxml2::XMLElement* outerControl = document.NewElement("Control"); - outerControl->SetAttribute("name", "sequential"); - - // Clone the existing element instead of deleting it - tinyxml2::XMLElement* clonedControl = innerControl->DeepClone(&document)->ToElement(); - - // Insert the cloned inner control inside the new outer control - outerControl->InsertEndChild(clonedControl); - - // Create and append the new element - tinyxml2::XMLElement* newEvent = document.NewElement("Event"); - newEvent->SetAttribute("name", "std_capabilities/FabricCompletionRunner"); - newEvent->SetAttribute("provider", "std_capabilities/FabricCompletionRunner"); - outerControl->InsertEndChild(newEvent); - - // Remove the original innerControl (after cloning) - plan->DeleteChild(innerControl); - - // Append the new outer control to - plan->InsertEndChild(outerControl); -} - -/** - * @brief check the plan for invalid/unsupported control and event tags - * uses recursive approach to go through the plan - * - * @param event EventClient used for logging and event publishing - * @param element XML Element to be evaluated - * @param events list containing valid event tags - * @param providers list containing providers - * @param control list containing valid control tags - * @param rejected list containing invalid tags - * - * @return `true` if element valid and supported and `false` otherwise - */ -bool check_tags(tinyxml2::XMLElement* element, std::vector& events, std::vector& providers, - std::vector& control, std::vector& rejected, std::string& error) -{ - const char* name; - const char* provider; - - std::string parameter_string; - convert_to_string(element, parameter_string); - - element->QueryStringAttribute("name", &name); - element->QueryStringAttribute("provider", &provider); - - std::string nametag; - std::string providertag; - std::string typetag(element->Name()); - - if (name) - nametag = name; - else - nametag = ""; - - if (provider) - providertag = provider; - else - providertag = ""; - - bool hasChildren = !element->NoChildren(); - bool hasSiblings = (element->NextSiblingElement() != nullptr); - bool foundInControl = xml_parser::search(control, nametag); - bool foundInEvents = xml_parser::search(events, nametag); - bool foundInProviders = xml_parser::search(providers, providertag); - bool returnValue = true; - - if (typetag == "Control") - { - if (!foundInControl) - { - error = "Control tag '" + nametag + "' not available in the valid list"; - rejected.push_back(parameter_string); - return false; - } - - if (hasChildren) - returnValue &= xml_parser::check_tags(element->FirstChildElement(), events, providers, control, rejected, error); - - if (hasSiblings) - returnValue &= xml_parser::check_tags(element->NextSiblingElement(), events, providers, control, rejected, error); - } - else if (typetag == "Event") - { - if (!foundInEvents || !foundInProviders) - { - error = "Event tag name '" + nametag + "' or provider '" + providertag + "' not available in the valid list"; - rejected.push_back(parameter_string); - return false; - } - - if (hasSiblings) - returnValue &= xml_parser::check_tags(element->NextSiblingElement(), events, providers, control, rejected, error); - } - else - { - error = "XML element is not valid :" + parameter_string; - rejected.push_back(parameter_string); - return false; - } - - return returnValue; -} - -/** - * @brief Returns control xml tags supported in extract_connections method - * - */ -std::vector get_control_list() -{ - std::vector tag_list; - - tag_list.push_back("sequential"); - tag_list.push_back("parallel"); - tag_list.push_back("recovery"); - - return tag_list; -} - -/** - * @brief parse through the plan and extract the connections - * - * @param element XML Element to be evaluated - * @param connections std::map containing extracted connections - * @param connection_id numerical id of the connection - * @param connection_type the type of connection - */ -int extract_connections(tinyxml2::XMLElement* element, std::map& connections, int connection_id = 0, - capabilities2::connection_type_t connection_type = capabilities2::connection_type_t::ON_SUCCESS) -{ - int predecessor_id; - - const char* name = element->Attribute("name"); - const char* provider = element->Attribute("provider"); - - std::string typetag(element->Name()); - - std::string nametag; - std::string providertag; - - if (name) - nametag = name; - else - nametag = ""; - - if (provider) - providertag = provider; - else - providertag = ""; - - bool hasChildren = (element->FirstChildElement() != nullptr); - bool hasSiblings = (element->NextSiblingElement() != nullptr); - - if (typetag == "Control") - { - if (nametag == "sequential") - { - if (hasChildren) - predecessor_id = - xml_parser::extract_connections(element->FirstChildElement(), connections, connection_id, capabilities2::connection_type_t::ON_SUCCESS); - } - else if (nametag == "parallel") - { - if (hasChildren) - predecessor_id = - xml_parser::extract_connections(element->FirstChildElement(), connections, connection_id, capabilities2::connection_type_t::ON_START); - } - else if (nametag == "recovery") - { - if (hasChildren) - predecessor_id = - xml_parser::extract_connections(element->FirstChildElement(), connections, connection_id, capabilities2::connection_type_t::ON_FAILURE); - } - - if (hasSiblings) - { - predecessor_id = xml_parser::extract_connections(element->NextSiblingElement(), connections, predecessor_id + 1, connection_type); - } - - return predecessor_id; - } - else if (typetag == "Event") - { - capabilities2::node_t node; - - node.source.runner = nametag; - node.source.provider = providertag; - node.source.parameters = element; - - predecessor_id = connection_id - 1; - - while (connections.count(connection_id) > 0) - connection_id += 1; - - connections[connection_id] = node; - - if (connection_id != 0) - { - if (connection_type == capabilities2::connection_type_t::ON_SUCCESS) - connections[predecessor_id].target_on_success = connections[connection_id].source; - - else if (connection_type == capabilities2::connection_type_t::ON_START) - connections[predecessor_id].target_on_start = connections[connection_id].source; - - else if (connection_type == capabilities2::connection_type_t::ON_FAILURE) - connections[predecessor_id].target_on_failure = connections[connection_id].source; - } - - if (hasSiblings) - predecessor_id = extract_connections(element->NextSiblingElement(), connections, connection_id + 1, connection_type); - else - predecessor_id += 1; // connection_id - - return predecessor_id; - } -} - -} // namespace xml_parser diff --git a/capabilities2_fabric/launch/fabric.launch.py b/capabilities2_fabric/launch/fabric.launch.py deleted file mode 100644 index 71493f1..0000000 --- a/capabilities2_fabric/launch/fabric.launch.py +++ /dev/null @@ -1,40 +0,0 @@ -''' -capabilities2_server launch file -''' - -import os -from launch import LaunchDescription -from launch_ros.actions import Node -from ament_index_python.packages import get_package_share_directory - - -def generate_launch_description(): - """Generate launch description for capabilities2 server - - Returns: - LaunchDescription: The launch description for capabilities2 executor - """ - # load config file - fabric_config = os.path.join(get_package_share_directory('capabilities2_fabric'), 'config', 'fabric.yaml') - - capabilities2_fabric = Node( - package='capabilities2_fabric', - namespace='', - executable='capabilities2_fabric', - name='capabilities2_fabric', - output='screen' - ) - - fabric_client = Node( - package='capabilities2_fabric', - namespace='', - executable='fabric_client', - name='fabric_client', - parameters=[fabric_config], - output='screen' - ) - - return LaunchDescription([ - capabilities2_fabric, - fabric_client - ]) diff --git a/capabilities2_fabric/launch/fabric_composed.launch.py b/capabilities2_fabric/launch/fabric_composed.launch.py deleted file mode 100644 index 6215018..0000000 --- a/capabilities2_fabric/launch/fabric_composed.launch.py +++ /dev/null @@ -1,49 +0,0 @@ -''' -capabilities2_server launch file -''' - -import os -from launch import LaunchDescription -from launch_ros.actions import Node -from launch_ros.actions import ComposableNodeContainer -from launch_ros.descriptions import ComposableNode -from ament_index_python.packages import get_package_share_directory - - -def generate_launch_description(): - """Generate launch description for capabilities2 server - - Returns: - LaunchDescription: The launch description for capabilities2 executor - """ - # load config file - fabric_config = os.path.join(get_package_share_directory('capabilities2_fabric'), 'config', 'fabric.yaml') - - # create bridge composition - fabric_container = ComposableNodeContainer( - name='capabilities2_fabric_container', - namespace='', - package='rclcpp_components', - executable='component_container_mt', - # prefix=['xterm -e gdb -ex run --args'], # Add GDB debugging prefix - arguments=['--ros-args', '--log-level', 'info'], - composable_node_descriptions=[ - ComposableNode( - package='capabilities2_fabric', - plugin='CapabilitiesFabric', - name='capabilities2_fabric', - output='screen' - ), - ComposableNode( - package='capabilities2_fabric', - plugin='CapabilitiesFabricClient', - name='fabric_client', - parameters=[fabric_config], - output='screen' - ) - ] - ) - - return LaunchDescription([ - fabric_container - ]) diff --git a/capabilities2_fabric/plans/default.xml b/capabilities2_fabric/plans/default.xml deleted file mode 100644 index a49f15b..0000000 --- a/capabilities2_fabric/plans/default.xml +++ /dev/null @@ -1,13 +0,0 @@ - - - - - - - - - - - - - \ No newline at end of file diff --git a/capabilities2_fabric/plans/navigation_1.xml b/capabilities2_fabric/plans/navigation_1.xml deleted file mode 100644 index 4dbae38..0000000 --- a/capabilities2_fabric/plans/navigation_1.xml +++ /dev/null @@ -1,6 +0,0 @@ - - - - - - \ No newline at end of file diff --git a/capabilities2_fabric/plans/navigation_2.xml b/capabilities2_fabric/plans/navigation_2.xml deleted file mode 100644 index 5c283d1..0000000 --- a/capabilities2_fabric/plans/navigation_2.xml +++ /dev/null @@ -1,7 +0,0 @@ - - - - - - - \ No newline at end of file diff --git a/capabilities2_fabric/plans/prompt_1.xml b/capabilities2_fabric/plans/prompt_1.xml deleted file mode 100644 index 8508f12..0000000 --- a/capabilities2_fabric/plans/prompt_1.xml +++ /dev/null @@ -1,7 +0,0 @@ - - - - - - - \ No newline at end of file diff --git a/capabilities2_fabric/plans/prompt_2.xml b/capabilities2_fabric/plans/prompt_2.xml deleted file mode 100644 index 1f33259..0000000 --- a/capabilities2_fabric/plans/prompt_2.xml +++ /dev/null @@ -1,7 +0,0 @@ - - - - - - - \ No newline at end of file diff --git a/capabilities2_fabric/plans/prompt_3.xml b/capabilities2_fabric/plans/prompt_3.xml deleted file mode 100644 index 6fc5494..0000000 --- a/capabilities2_fabric/plans/prompt_3.xml +++ /dev/null @@ -1,8 +0,0 @@ - - - - - - - - \ No newline at end of file diff --git a/capabilities2_fabric/plans/prompt_4.xml b/capabilities2_fabric/plans/prompt_4.xml deleted file mode 100644 index dde90ac..0000000 --- a/capabilities2_fabric/plans/prompt_4.xml +++ /dev/null @@ -1,9 +0,0 @@ - - - - - - - - - \ No newline at end of file diff --git a/capabilities2_fabric/readme.md b/capabilities2_fabric/readme.md deleted file mode 100644 index 3bfeb86..0000000 --- a/capabilities2_fabric/readme.md +++ /dev/null @@ -1,102 +0,0 @@ -# Capabilities2_fabric - -Capabilities2_fabric is a ROS2 package that provides a system to coordinate and manage various capabilities as defined by the Capabilities2 framework. This package extends the functionality of the Capabilities2 package to implement a control framework based on capabilities. It is designed to parse an execution plan given via an XML file and then to identify connections between various capabilities in the system. - -Currently the system support 3 types of Control fuctions `sequential`, `parallel` and `recovery`, and multitude of Event functions. - -## Features - -- Dynamic Capability Loading: Interacts with and manages capabilities defined by the capabilities2 framework. -- Flexible Workflow Execution: Parses XML-based plans and identifies event-driven callbacks for success, failure, or in-progress states. - - -## Launching capabilities2_fabric - -`capabilities2_fabric/plans` folder includes sample XML plans that can be used to test the system. New plans can be added to the same folder or a different location. - -Then modify the `capabilities2_fabric/config/fabric.yaml` file to change the active execution plan. -A number of plans are availabe with the package and included in the `fabric.yaml` file that has been commented out. Uncomment them to use. Make sure to leave only one line uncommented. - -```yaml -/**: - ros__parameters: - plan_file_path: "install/capabilities2_fabric/share/capabilities2_fabric/plans/default.xml" - -``` -Finally start the capabilities2 server. Run the following on a new terminal - -```bash -source install/setup.bash -ros2 launch capabilities2_fabric fabric.launch.py -``` - - -## XML Plan Parsing - -The capabilities2_fabric package relies on XML-based plans to define workflows. These plans specify the sequence of capabilities to execute, along with the associated parameters. The XML format includes tags for capabilities as events, and control flows enabling complex workflows to be structured in a modular way. - -Below is an example XML plan for configuring a set of capabilities: - -```xml - - - - - - - - - - - - - - - - - - - - - - - - - -``` - -## API - -| Node | Description | -| :--- | :--- | -| `capabilities2_Fabric` | Implemented the XML parsing and connection identification as well as communicating with `capabilities_server` to configure capability events | -| `capabilities2_File_Parser` | Reads an exection plan from a given path and sends it to the `capabilities2_fabric` node. Can be used as a sample action client to work with the `capabilities2_fabric` | - -| Action | Action Message | Description | -| :--- | :--- | :--- | -| `~/capabilities_fabric` | `Plan.action` | Receive and XML plan via the message for execution| - -## Samples and Testing - -### Navigation - -1. [WaypointRunner Example 1](./docs/waypoint_runner_ex1.md) -Implements at the very basic fabric triggering that moves the robot from one point to another. - -2. [WaypointRunner Example 2](./docs/waypoint_runner_ex2.md) -Implements navigating through 2 points using 'sequential' control functionality. - - -### Prompting - -1. [PromptCapabilityRunner Example](./docs/prompt_capability_runner_ex1.md) -Implements requesting for robot's capabilities and prompting them to the LLM - -2. [PromptOccupancyRunner Example](./docs/prompt_occupancy_runner_ex1.md) -Implements listening for robot's occupancy grid and prompting them to the LLM - -2. [PromptPoseRunner Example](./docs/prompt_pose_runner_ex1.md) -Implements listening for robot's pose and prompting them to the LLM - -2. [PromptPlanRunner Example](./docs/prompt_plan_runner_ex1.md) -Implements prompting the LLM for a plan for a new task and setting it to Capabilities Fabric diff --git a/capabilities2_fabric/src/capabilities_fabric_client_component.cpp b/capabilities2_fabric/src/capabilities_fabric_client_component.cpp deleted file mode 100644 index 9189005..0000000 --- a/capabilities2_fabric/src/capabilities_fabric_client_component.cpp +++ /dev/null @@ -1,4 +0,0 @@ -#include -#include - -RCLCPP_COMPONENTS_REGISTER_NODE(CapabilitiesFabricClient) diff --git a/capabilities2_fabric/src/capabilities_fabric_client_node.cpp b/capabilities2_fabric/src/capabilities_fabric_client_node.cpp deleted file mode 100644 index f10f3c9..0000000 --- a/capabilities2_fabric/src/capabilities_fabric_client_node.cpp +++ /dev/null @@ -1,20 +0,0 @@ -#include - -int main(int argc, char* argv[]) -{ - // Initialize the ROS 2 C++ client library - rclcpp::init(argc, argv); - - // Create a shared pointer to the CapabilitiesFabricClient - auto parser_node = std::make_shared(); - - // Initialize the node - parser_node->initialize(); // Call initialize after construction - - // Spin the node to process callbacks - rclcpp::spin(parser_node); - - rclcpp::shutdown(); - - return 0; -} diff --git a/capabilities2_fabric/src/capabilities_fabric_component.cpp b/capabilities2_fabric/src/capabilities_fabric_component.cpp deleted file mode 100644 index 0219a1e..0000000 --- a/capabilities2_fabric/src/capabilities_fabric_component.cpp +++ /dev/null @@ -1,4 +0,0 @@ -#include -#include - -RCLCPP_COMPONENTS_REGISTER_NODE(CapabilitiesFabric) \ No newline at end of file diff --git a/capabilities2_fabric/src/capabilities_fabric_node.cpp b/capabilities2_fabric/src/capabilities_fabric_node.cpp deleted file mode 100644 index e89ccbb..0000000 --- a/capabilities2_fabric/src/capabilities_fabric_node.cpp +++ /dev/null @@ -1,17 +0,0 @@ -#include - -int main(int argc, char** argv) -{ - rclcpp::init(argc, argv); - - // Create the node instance - auto node = std::make_shared(); - - // Initialize the node components after construction - node->initialize(); - - rclcpp::spin(node); - - rclcpp::shutdown(); - return 0; -} \ No newline at end of file diff --git a/capabilities2_msgs/CMakeLists.txt b/capabilities2_msgs/CMakeLists.txt index 0d19c29..0fbf54e 100644 --- a/capabilities2_msgs/CMakeLists.txt +++ b/capabilities2_msgs/CMakeLists.txt @@ -43,16 +43,11 @@ set(srv_files "srv/ConfigureCapability.srv" "srv/TriggerCapability.srv" "srv/Launch.srv" - "srv/GetFabricStatus.srv" - "srv/SetFabricPlan.srv" - "srv/CancelFabricPlan.srv" - "srv/CompleteFabric.srv" ) set(action_files "action/Capability.action" "action/Launch.action" - "action/Plan.action" ) rosidl_generate_interfaces(${PROJECT_NAME} diff --git a/capabilities2_msgs/action/Plan.action b/capabilities2_msgs/action/Plan.action deleted file mode 100644 index ab827c1..0000000 --- a/capabilities2_msgs/action/Plan.action +++ /dev/null @@ -1,14 +0,0 @@ -# action to transfer a plan for execution -# goal -std_msgs/Header header -string plan ---- -# result -std_msgs/Header header -bool success -string message -string[] failed_elements ---- -# feedback -std_msgs/Header header -string progress diff --git a/capabilities2_msgs/srv/CancelFabricPlan.srv b/capabilities2_msgs/srv/CancelFabricPlan.srv deleted file mode 100644 index 25a4b22..0000000 --- a/capabilities2_msgs/srv/CancelFabricPlan.srv +++ /dev/null @@ -1,4 +0,0 @@ -std_msgs/Header header ---- -std_msgs/Header header -bool success \ No newline at end of file diff --git a/capabilities2_msgs/srv/CompleteFabric.srv b/capabilities2_msgs/srv/CompleteFabric.srv deleted file mode 100644 index 73b314f..0000000 --- a/capabilities2_msgs/srv/CompleteFabric.srv +++ /dev/null @@ -1 +0,0 @@ ---- \ No newline at end of file diff --git a/capabilities2_msgs/srv/GetFabricStatus.srv b/capabilities2_msgs/srv/GetFabricStatus.srv deleted file mode 100644 index 675f947..0000000 --- a/capabilities2_msgs/srv/GetFabricStatus.srv +++ /dev/null @@ -1,12 +0,0 @@ -std_msgs/Header header ---- -std_msgs/Header header -uint8 status -uint8 FABRIC_IDLE=0 -uint8 FABRIC_RUNNING=1 -uint8 FABRIC_CANCELED=2 -uint8 FABRIC_ABORTED=3 -uint8 FABRIC_FAILED=4 -uint8 FABRIC_LAUNCHED=5 -uint8 FABRIC_COMPLETED=6 -uint8 UNKNOWN=7 \ No newline at end of file diff --git a/capabilities2_msgs/srv/SetFabricPlan.srv b/capabilities2_msgs/srv/SetFabricPlan.srv deleted file mode 100644 index 49d3223..0000000 --- a/capabilities2_msgs/srv/SetFabricPlan.srv +++ /dev/null @@ -1,5 +0,0 @@ -std_msgs/Header header -string plan ---- -std_msgs/Header header -bool success \ No newline at end of file diff --git a/capabilities2_runner_capabilities/include/capabilities2_runner_capabilities/capability_get_runner.hpp b/capabilities2_runner_capabilities/include/capabilities2_runner_capabilities/get_runner.hpp similarity index 100% rename from capabilities2_runner_capabilities/include/capabilities2_runner_capabilities/capability_get_runner.hpp rename to capabilities2_runner_capabilities/include/capabilities2_runner_capabilities/get_runner.hpp diff --git a/capabilities2_runner_capabilities/plugins.xml b/capabilities2_runner_capabilities/plugins.xml index 32fd1e9..f4ff0f8 100644 --- a/capabilities2_runner_capabilities/plugins.xml +++ b/capabilities2_runner_capabilities/plugins.xml @@ -4,14 +4,4 @@ A plugin that request capabilities from the capabilities server and transfers to an LLM - - - A plugin that notifies about the completion of the fabric to the action server - - - - - A plugin that sets a new Fabric plan to the Fabric - - diff --git a/capabilities2_runner_capabilities/src/capabilities2_runner.cpp b/capabilities2_runner_capabilities/src/capabilities2_runner.cpp index 39c18de..c6c35a5 100644 --- a/capabilities2_runner_capabilities/src/capabilities2_runner.cpp +++ b/capabilities2_runner_capabilities/src/capabilities2_runner.cpp @@ -1,10 +1,6 @@ #include #include -#include -#include -#include +#include // register runner plugins -PLUGINLIB_EXPORT_CLASS(capabilities2_runner::FabricCompletionRunner, capabilities2_runner::RunnerBase) -PLUGINLIB_EXPORT_CLASS(capabilities2_runner::FabricSetPlanRunner, capabilities2_runner::RunnerBase) PLUGINLIB_EXPORT_CLASS(capabilities2_runner::CapabilityGetRunner, capabilities2_runner::RunnerBase) diff --git a/capabilities2_fabric/.clang-format b/capabilities2_runner_fabric/.clang-format similarity index 99% rename from capabilities2_fabric/.clang-format rename to capabilities2_runner_fabric/.clang-format index 92effdd..d36804f 100644 --- a/capabilities2_fabric/.clang-format +++ b/capabilities2_runner_fabric/.clang-format @@ -14,7 +14,7 @@ BreakBeforeBinaryOperators: false BreakBeforeTernaryOperators: false BreakConstructorInitializersBeforeComma: true BinPackParameters: true -ColumnLimit: 150 +ColumnLimit: 120 ConstructorInitializerAllOnOneLineOrOnePerLine: true DerivePointerBinding: false PointerBindsToType: true diff --git a/capabilities2_runner_fabric/CMakeLists.txt b/capabilities2_runner_fabric/CMakeLists.txt new file mode 100644 index 0000000..f8caf64 --- /dev/null +++ b/capabilities2_runner_fabric/CMakeLists.txt @@ -0,0 +1,59 @@ +cmake_minimum_required(VERSION 3.8) +project(capabilities2_runner_fabric) + +# Default to C++17 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_action REQUIRED) +find_package(pluginlib REQUIRED) +find_package(fabric_msgs REQUIRED) +find_package(capabilities2_msgs REQUIRED) +find_package(capabilities2_runner REQUIRED) +find_package(capabilities2_events REQUIRED) +find_package(tinyxml2_vendor REQUIRED) +find_package(TinyXML2 REQUIRED) # provided by tinyxml2 upstream, or tinyxml2_vendor + +include_directories( + include +) + +add_library(${PROJECT_NAME} SHARED + src/fabric_runners.cpp +) + +ament_target_dependencies(${PROJECT_NAME} + rclcpp + rclcpp_action + pluginlib + fabric_msgs + capabilities2_msgs + capabilities2_runner + capabilities2_events + TinyXML2 +) + +pluginlib_export_plugin_description_file(capabilities2_runner plugins.xml) + +install(TARGETS ${PROJECT_NAME} + EXPORT ${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +install(DIRECTORY include/ + DESTINATION include +) + +ament_export_include_directories(include) +ament_export_libraries(${PROJECT_NAME}) + +ament_package() diff --git a/capabilities2_runner_capabilities/include/capabilities2_runner_capabilities/fabric_completion_runner.hpp b/capabilities2_runner_fabric/include/capabilities2_runner_fabric/completion_runner.hpp similarity index 74% rename from capabilities2_runner_capabilities/include/capabilities2_runner_capabilities/fabric_completion_runner.hpp rename to capabilities2_runner_fabric/include/capabilities2_runner_fabric/completion_runner.hpp index c07d7ca..2475636 100644 --- a/capabilities2_runner_capabilities/include/capabilities2_runner_capabilities/fabric_completion_runner.hpp +++ b/capabilities2_runner_fabric/include/capabilities2_runner_fabric/completion_runner.hpp @@ -3,7 +3,7 @@ #include #include #include -#include +#include namespace capabilities2_runner { @@ -14,7 +14,7 @@ namespace capabilities2_runner * call on the /capabilities_fabric/set_completion service, providing it as a * capability that notifys the completion of the fabric */ -class FabricCompletionRunner : public ServiceRunner +class FabricCompletionRunner : public ServiceRunner { public: FabricCompletionRunner() : ServiceRunner() @@ -29,7 +29,7 @@ class FabricCompletionRunner : public ServiceRunner #include -#include +#include namespace capabilities2_runner { @@ -18,7 +18,7 @@ namespace capabilities2_runner * Class to run capabilities2 executor action based capability * */ -class FabricSetPlanRunner : public ServiceRunner +class FabricSetPlanRunner : public ServiceRunner { public: FabricSetPlanRunner() : ServiceRunner() @@ -33,7 +33,7 @@ class FabricSetPlanRunner : public ServiceRunnerFirstChildElement("ReceievdPlan"); - capabilities2_msgs::srv::SetFabricPlan::Request request; + fabric_msgs::srv::SetFabricPlan::Request request; // Check if the element was found and has text content if (planElement && planElement->GetText()) diff --git a/capabilities2_fabric/package.xml b/capabilities2_runner_fabric/package.xml similarity index 75% rename from capabilities2_fabric/package.xml rename to capabilities2_runner_fabric/package.xml index 45bb847..3be6c38 100644 --- a/capabilities2_fabric/package.xml +++ b/capabilities2_runner_fabric/package.xml @@ -1,25 +1,25 @@ - capabilities2_fabric + capabilities2_runner_fabric 0.0.0 TODO: Package description kalana - - MIT + TODO: License declaration ament_cmake + ament_lint_auto + ament_lint_common + rclcpp rclcpp_action - capabilities2_msgs + pluginlib + std_msgs + fabric_msgs + capabilities2_runner capabilities2_events - capabilities2_utils tinyxml2_vendor - backward_ros - - ament_lint_auto - ament_lint_common ament_cmake diff --git a/capabilities2_runner_fabric/plugins.xml b/capabilities2_runner_fabric/plugins.xml new file mode 100644 index 0000000..5168f4b --- /dev/null +++ b/capabilities2_runner_fabric/plugins.xml @@ -0,0 +1,12 @@ + + + + A plugin that notifies about the completion of the fabric to the action server + + + + + A plugin that sets a new Fabric plan to the Fabric + + + diff --git a/capabilities2_runner_fabric/src/fabric_runners.cpp b/capabilities2_runner_fabric/src/fabric_runners.cpp new file mode 100644 index 0000000..6167db2 --- /dev/null +++ b/capabilities2_runner_fabric/src/fabric_runners.cpp @@ -0,0 +1,8 @@ +#include +#include +#include +#include + +// register runner plugins +PLUGINLIB_EXPORT_CLASS(capabilities2_runner::FabricCompletionRunner, capabilities2_runner::RunnerBase) +PLUGINLIB_EXPORT_CLASS(capabilities2_runner::FabricSetPlanRunner, capabilities2_runner::RunnerBase) From d90596b748da92b2cd3771ac784310d03e15adae Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Wed, 7 May 2025 18:13:39 +1000 Subject: [PATCH 109/133] updated documentation --- AUTHORS | 2 +- readme.md | 14 ++++---------- 2 files changed, 5 insertions(+), 11 deletions(-) diff --git a/AUTHORS b/AUTHORS index 848e7ac..c86be4e 100644 --- a/AUTHORS +++ b/AUTHORS @@ -1,4 +1,4 @@ Michael Pritchard Thomas Muller-dardelin -Kalana Rathnayake +Kalana Ratnayake Buddhi Gamage diff --git a/readme.md b/readme.md index 9d0c9d9..7da78ab 100644 --- a/readme.md +++ b/readme.md @@ -9,7 +9,10 @@ A reimplementation of the [capabilities](https://github.com/osrf/capabilities) package. This package is implemented using C++17 and extends the capabilities package features. - [capabilities2_server](./capabilities2_server/readme.md) package contains the core of the system. - [capabilities2_runner](./capabilities2_server/readme.md) package contains base and template classes for capability implementations. -- [capabilities2_fabric](./capabilities2_fabric/readme.md) package implements a control framework that utlizes capabilities2 system. + + +## Extentions +- [Fabric](https://github.com/CollaborativeRoboticsLab/fabric) package implements a behaviour planning framework that utlizes capabilities2 system. ## System structure @@ -44,15 +47,6 @@ source install/setup.bash ros2 launch capabilities2_server server.launch.py ``` -### Starting the Capabilities2 Fabric - -Start the capabilities2 server first. Then run the following on a new terminal - -```bash -source install/setup.bash -ros2 launch capabilities2_fabric fabric.launch.py -``` - ## Additional Information - [Motivation and Example Use Cases](./docs/motivation_and_examples.md) - [Design Information](./docs/design.md) From 0e975f5feb7eb2aa813920d04209706b8f56958e Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Wed, 7 May 2025 18:44:10 +1000 Subject: [PATCH 110/133] tested --- .../capabilities2_runner/runner_base.hpp | 6 +-- .../capabilities2_utils/bond_client.hpp | 39 ++++++++++++++++--- 2 files changed, 37 insertions(+), 8 deletions(-) diff --git a/capabilities2_runner/include/capabilities2_runner/runner_base.hpp b/capabilities2_runner/include/capabilities2_runner/runner_base.hpp index 2c9bbdf..f9e702f 100644 --- a/capabilities2_runner/include/capabilities2_runner/runner_base.hpp +++ b/capabilities2_runner/include/capabilities2_runner/runner_base.hpp @@ -453,7 +453,7 @@ class RunnerBase auto message = Event(); message.header.stamp = node_->now(); - message.origin_node = "capability_runners"; + message.origin_node = "runners"; message.source.capability = run_config_.interface; message.source.provider = run_config_.provider; message.target.capability = target_capability; @@ -493,7 +493,7 @@ class RunnerBase auto message = Event(); message.header.stamp = node_->now(); - message.origin_node = "capability_runners"; + message.origin_node = "runners"; message.source.capability = run_config_.interface; message.source.provider = run_config_.provider; message.target.capability = ""; @@ -512,7 +512,7 @@ class RunnerBase auto message = Event(); message.header.stamp = node_->now(); - message.origin_node = "capability_runners"; + message.origin_node = "runners"; message.source.capability = run_config_.interface; message.source.provider = run_config_.provider; message.target.capability = ""; diff --git a/capabilities2_utils/include/capabilities2_utils/bond_client.hpp b/capabilities2_utils/include/capabilities2_utils/bond_client.hpp index de20de9..ef7d675 100644 --- a/capabilities2_utils/include/capabilities2_utils/bond_client.hpp +++ b/capabilities2_utils/include/capabilities2_utils/bond_client.hpp @@ -2,20 +2,34 @@ #include #include #include +#include class BondClient { public: - BondClient(rclcpp::Node::SharedPtr node, const std::string& bond_id, const std::string& bonds_topic = "/capabilities/bond") + /** + * @brief Construct a new Bond Client object + * + * @param node pointer to the node + * @param event_client pointer to the event client + * @param bond_id Bond id string + * @param bonds_topic Bond topic to be published + */ + BondClient(rclcpp::Node::SharedPtr node, std::shared_ptr event_client, const std::string &bond_id, const std::string &bonds_topic = "/capabilities/bond") { topic_ = bonds_topic; bond_id_ = bond_id; node_ = node; + event_ = event_client; } + /** + * @brief start the bond + * + */ void start() { - RCLCPP_INFO(node_->get_logger(), "[BondClient] creating bond to capabilities server"); + event_->info("[BondClient] creating bond to capabilities server"); bond_ = std::make_unique(topic_, bond_id_, node_, std::bind(&BondClient::on_broken, this), std::bind(&BondClient::on_formed, this)); @@ -24,9 +38,13 @@ class BondClient bond_->start(); } + /** + * @brief stop the bond + * + */ void stop() { - RCLCPP_INFO(node_->get_logger(), "[BondClient] destroying bond to capabilities server"); + event_->info("[BondClient] destroying bond to capabilities server"); if (bond_) { @@ -40,16 +58,24 @@ class BondClient } private: + /** + * @brief callback function for bond formed event + * + */ void on_formed() { // log bond established event - RCLCPP_INFO(node_->get_logger(), "[BondClient] bond with capabilities server formed with id: %s", bond_id_.c_str()); + event_->info("[BondClient] bond with capabilities server formed with id: " + bond_id_); } + /** + * @brief callback function for bond broken event + * + */ void on_broken() { // log bond established event - RCLCPP_INFO(node_->get_logger(), "[BondClient] bond with capabilities server broken with id: %s", bond_id_.c_str()); + event_->info("[BondClient] bond with capabilities server broken with id: " + bond_id_); } /** Ros node pointer */ @@ -63,4 +89,7 @@ class BondClient /** Heart beat bond with capabilities server */ std::shared_ptr bond_; + + /** Event client for publishing events */ + std::shared_ptr event_; }; \ No newline at end of file From 25f737157d150627b550cef08d5a24b27c4510b0 Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Thu, 8 May 2025 00:48:55 +1000 Subject: [PATCH 111/133] added foxglove dependencies and information --- .../launch/foxglove.launch.py | 60 +++++++++++++++++++ capabilities2_events/package.xml | 1 + capabilities2_server/launch/server.launch.py | 10 +++- docs/foxglove_studio.md | 7 +++ readme.md | 1 + 5 files changed, 78 insertions(+), 1 deletion(-) create mode 100644 capabilities2_events/launch/foxglove.launch.py create mode 100644 docs/foxglove_studio.md diff --git a/capabilities2_events/launch/foxglove.launch.py b/capabilities2_events/launch/foxglove.launch.py new file mode 100644 index 0000000..82d191a --- /dev/null +++ b/capabilities2_events/launch/foxglove.launch.py @@ -0,0 +1,60 @@ +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.substitutions import LaunchConfiguration +from launch.actions import DeclareLaunchArgument + + +def generate_launch_description(): + # Declare launch arguments + launch_args = [ + DeclareLaunchArgument('port', default_value='8765'), + DeclareLaunchArgument('address', default_value='0.0.0.0'), + DeclareLaunchArgument('tls', default_value='false'), + DeclareLaunchArgument('certfile', default_value=''), + DeclareLaunchArgument('keyfile', default_value=''), + DeclareLaunchArgument('topic_whitelist', default_value="['.*']"), + DeclareLaunchArgument('param_whitelist', default_value="['.*']"), + DeclareLaunchArgument('service_whitelist', default_value="['.*']"), + DeclareLaunchArgument('client_topic_whitelist', default_value="['.*']"), + DeclareLaunchArgument('min_qos_depth', default_value='1'), + DeclareLaunchArgument('max_qos_depth', default_value='10'), + DeclareLaunchArgument('num_threads', default_value='0'), + DeclareLaunchArgument('send_buffer_limit', default_value='10000000'), + DeclareLaunchArgument('use_sim_time', default_value='false'), + DeclareLaunchArgument('capabilities', default_value="[clientPublish,parameters,parametersSubscribe,services,connectionGraph,assets]"), + DeclareLaunchArgument('include_hidden', default_value='false'), + DeclareLaunchArgument( + 'asset_uri_allowlist', + default_value=r"['^package://(?:[-\\w]+/)*[-\\w]+\\.(?:dae|fbx|glb|gltf|jpeg|jpg|mtl|obj|png|stl|tif|tiff|urdf|webp|xacro)$']" + ), + DeclareLaunchArgument('ignore_unresponsive_param_nodes', default_value='true'), + ] + + # Define the Node + foxglove_bridge_node = Node( + package='foxglove_bridge', + executable='foxglove_bridge', + parameters=[{ + 'port': LaunchConfiguration('port'), + 'address': LaunchConfiguration('address'), + 'tls': LaunchConfiguration('tls'), + 'certfile': LaunchConfiguration('certfile'), + 'keyfile': LaunchConfiguration('keyfile'), + 'topic_whitelist': LaunchConfiguration('topic_whitelist'), + 'param_whitelist': LaunchConfiguration('param_whitelist'), + 'service_whitelist': LaunchConfiguration('service_whitelist'), + 'client_topic_whitelist': LaunchConfiguration('client_topic_whitelist'), + 'min_qos_depth': LaunchConfiguration('min_qos_depth'), + 'max_qos_depth': LaunchConfiguration('max_qos_depth'), + 'num_threads': LaunchConfiguration('num_threads'), + 'send_buffer_limit': LaunchConfiguration('send_buffer_limit'), + 'use_sim_time': LaunchConfiguration('use_sim_time'), + 'capabilities': LaunchConfiguration('capabilities'), + 'include_hidden': LaunchConfiguration('include_hidden'), + 'asset_uri_allowlist': LaunchConfiguration('asset_uri_allowlist'), + 'ignore_unresponsive_param_nodes': LaunchConfiguration('ignore_unresponsive_param_nodes'), + }], + output='screen' + ) + + return LaunchDescription(launch_args + [foxglove_bridge_node]) \ No newline at end of file diff --git a/capabilities2_events/package.xml b/capabilities2_events/package.xml index 9674c1a..c3ff556 100644 --- a/capabilities2_events/package.xml +++ b/capabilities2_events/package.xml @@ -15,6 +15,7 @@ rclcpp capabilities2_msgs rclcpp_components + foxglove_bridge ament_cmake diff --git a/capabilities2_server/launch/server.launch.py b/capabilities2_server/launch/server.launch.py index 6879e11..1febf65 100644 --- a/capabilities2_server/launch/server.launch.py +++ b/capabilities2_server/launch/server.launch.py @@ -5,6 +5,8 @@ import os from launch import LaunchDescription from launch_ros.actions import Node +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource from ament_index_python.packages import get_package_share_directory @@ -16,6 +18,7 @@ def generate_launch_description(): """ # load config file server_config = os.path.join(get_package_share_directory('capabilities2_server'), 'config', 'capabilities.yaml') + foxglove_path = os.path.join(get_package_share_directory('capabilities2_events'), 'launch', 'foxglove.launch.py') # create bridge composition capabilities2 = Node( @@ -27,7 +30,12 @@ def generate_launch_description(): arguments=['--ros-args', '--log-level', 'info'] ) + foxglove_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource(foxglove_path), + ) + # return return LaunchDescription([ - capabilities2 + capabilities2, + foxglove_launch ]) diff --git a/docs/foxglove_studio.md b/docs/foxglove_studio.md new file mode 100644 index 0000000..341ea13 --- /dev/null +++ b/docs/foxglove_studio.md @@ -0,0 +1,7 @@ +# Dependency Installation with Foxglove Studio + +We utilize foxglove studio to visalize information about the Capabilities2 system. + +Event system has been defined in capabilities2_events package and foxglove-bridge has been set as a dependency of capabilities2_events for ease of installation. We have selected foxglove-bridge over rosbridge due to performance improvements foxglove-bridge has over rosbridge due to former being written in C++ and latter being in Python. + +To visualize data, download foxglove-studio from [website](https://foxglove.dev/download) and create a free account when signing in. \ No newline at end of file diff --git a/readme.md b/readme.md index 7da78ab..99ac6dc 100644 --- a/readme.md +++ b/readme.md @@ -37,6 +37,7 @@ Runners can be created using the runner API parent classes [here](./capabilities - [Setup Instructions without devcontainer](./docs/setup.md) - [Dependency installation for Nav2 Runners](./docs/nav2_setup.md) - [Dependency installation for Prompt Runners](./docs/prompt_tools_setup.md) +- [Dependency installation for Foxglove-studio](./docs/foxglove_studio.md) ## Quick Startup information From 9bd6059e340ebe89a6fae3e33f547876eec6aef3 Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Fri, 9 May 2025 12:36:58 +1000 Subject: [PATCH 112/133] minor modifications --- capabilities2_server/launch/server.launch.py | 8 +------- 1 file changed, 1 insertion(+), 7 deletions(-) diff --git a/capabilities2_server/launch/server.launch.py b/capabilities2_server/launch/server.launch.py index 1febf65..79dc98c 100644 --- a/capabilities2_server/launch/server.launch.py +++ b/capabilities2_server/launch/server.launch.py @@ -18,7 +18,6 @@ def generate_launch_description(): """ # load config file server_config = os.path.join(get_package_share_directory('capabilities2_server'), 'config', 'capabilities.yaml') - foxglove_path = os.path.join(get_package_share_directory('capabilities2_events'), 'launch', 'foxglove.launch.py') # create bridge composition capabilities2 = Node( @@ -30,12 +29,7 @@ def generate_launch_description(): arguments=['--ros-args', '--log-level', 'info'] ) - foxglove_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource(foxglove_path), - ) - # return return LaunchDescription([ - capabilities2, - foxglove_launch + capabilities2 ]) From 49d74b60e47ea7f644cd582dd9dcf1838a2e209a Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Mon, 12 May 2025 00:51:33 +1000 Subject: [PATCH 113/133] minor change --- capabilities2_events/launch/foxglove.launch.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/capabilities2_events/launch/foxglove.launch.py b/capabilities2_events/launch/foxglove.launch.py index 82d191a..80176f0 100644 --- a/capabilities2_events/launch/foxglove.launch.py +++ b/capabilities2_events/launch/foxglove.launch.py @@ -12,7 +12,8 @@ def generate_launch_description(): DeclareLaunchArgument('tls', default_value='false'), DeclareLaunchArgument('certfile', default_value=''), DeclareLaunchArgument('keyfile', default_value=''), - DeclareLaunchArgument('topic_whitelist', default_value="['.*']"), + DeclareLaunchArgument('topic_whitelist', default_value="['/events']"), + # DeclareLaunchArgument('topic_whitelist', default_value="['.*']"), # Uncomment this line to whitelist all topics DeclareLaunchArgument('param_whitelist', default_value="['.*']"), DeclareLaunchArgument('service_whitelist', default_value="['.*']"), DeclareLaunchArgument('client_topic_whitelist', default_value="['.*']"), From b7b730f00f9a249bae34710f12ed4f27cceb69f5 Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Tue, 13 May 2025 11:48:22 +1000 Subject: [PATCH 114/133] minor updates to capabilities --- .../capabilities2_events/event_client.hpp | 27 ++++++++++++++++++- .../capabilities2_events/event_listener.hpp | 26 +++++++++++++++--- capabilities2_msgs/msg/CapabilityEvent.msg | 9 ++++--- .../capabilities_server.hpp | 8 ++++++ .../capabilities2_server/runner_cache.hpp | 5 ---- 5 files changed, 62 insertions(+), 13 deletions(-) diff --git a/capabilities2_events/include/capabilities2_events/event_client.hpp b/capabilities2_events/include/capabilities2_events/event_client.hpp index 742a262..e87a433 100644 --- a/capabilities2_events/include/capabilities2_events/event_client.hpp +++ b/capabilities2_events/include/capabilities2_events/event_client.hpp @@ -154,7 +154,7 @@ class EventClient } /** - * @brief publishes status information to the given topic as error with + * @brief publishes status information to the given topic as error with * element infromation included * * @param message Message to be published @@ -164,6 +164,31 @@ class EventClient event_publisher_->publish(message); } + /** + * @brief publishes element information to the given topic as error + * + * @param element element information to be published + */ + void define_event(const std::string& src_capability, const std::string& src_provider, const std::string& tgt_capability, + const std::string& tgt_provider, uint8_t event = Event::UNDEFINED) + { + auto message = Event(); + + message.header.stamp = node_->now(); + message.origin_node = node_name_; + message.source.capability = src_capability; + message.source.provider = src_provider; + message.target.capability = tgt_capability; + message.target.provider = tgt_provider; + message.thread_id = -1; + message.event = event; + message.type = Event::DEFINE_EVENT; + message.content = ""; + message.pid = -1; + + event_publisher_->publish(message); + } + protected: /** * @brief Node pointer to access logging interface diff --git a/capabilities2_events/include/capabilities2_events/event_listener.hpp b/capabilities2_events/include/capabilities2_events/event_listener.hpp index 4099719..fadb4c0 100644 --- a/capabilities2_events/include/capabilities2_events/event_listener.hpp +++ b/capabilities2_events/include/capabilities2_events/event_listener.hpp @@ -26,6 +26,22 @@ class CapabilitiesEventListener : public rclcpp::Node { text = "[" + msg.origin_node + "]" + "[" + msg.source.capability + "/" + std::to_string(msg.thread_id) + "] " + msg.content; } + else if (msg.type == Event::DEFINE_EVENT and msg.event == Event::STARTED) + { + text = "[" + msg.origin_node + "]" + "[" + msg.source.capability + "] will trigger [" + msg.target.capability + "] on start"; + } + else if (msg.type == Event::DEFINE_EVENT and msg.event == Event::STOPPED) + { + text = "[" + msg.origin_node + "]" + "[" + msg.source.capability + "] will trigger [" + msg.target.capability + "] on stop"; + } + else if (msg.type == Event::DEFINE_EVENT and msg.event == Event::FAILED) + { + text = "[" + msg.origin_node + "]" + "[" + msg.source.capability + "] will trigger [" + msg.target.capability + "] on failure"; + } + else if (msg.type == Event::DEFINE_EVENT and msg.event == Event::SUCCEEDED) + { + text = "[" + msg.origin_node + "]" + "[" + msg.source.capability + "] will trigger [" + msg.target.capability + "] on success"; + } else if (msg.thread_id >= 0 and msg.target.capability == "" and msg.source.capability == "") { text = "[" + msg.origin_node + "]" + "[" + std::to_string(msg.thread_id) + "] " + msg.content; @@ -54,12 +70,16 @@ class CapabilitiesEventListener : public rclcpp::Node if (msg.type == Event::ERROR) RCLCPP_ERROR(get_logger(), text.c_str()); - else if (msg.type == Event::INFO) - RCLCPP_INFO(get_logger(), text.c_str()); + else if (msg.type == Event::ERROR_ELEMENT) + RCLCPP_ERROR(get_logger(), text.c_str()); else if (msg.type == Event::DEBUG) RCLCPP_DEBUG(get_logger(), text.c_str()); + else if (msg.type == Event::INFO) + RCLCPP_INFO(get_logger(), text.c_str()); + else if (msg.type == Event::DEFINE_EVENT) + RCLCPP_INFO(get_logger(), text.c_str()); else - RCLCPP_ERROR(get_logger(), text.c_str()); + RCLCPP_INFO(get_logger(), text.c_str()); } rclcpp::Subscription::SharedPtr subscription_; diff --git a/capabilities2_msgs/msg/CapabilityEvent.msg b/capabilities2_msgs/msg/CapabilityEvent.msg index cd7d38b..e5adef1 100644 --- a/capabilities2_msgs/msg/CapabilityEvent.msg +++ b/capabilities2_msgs/msg/CapabilityEvent.msg @@ -14,10 +14,10 @@ int8 thread_id # Events available uint8 IDLE=0 -uint8 STARTED=1 -uint8 STOPPED=2 -uint8 FAILED=3 -uint8 SUCCEEDED=4 +uint8 STARTED=1 #on_started +uint8 STOPPED=2 #on_stopped +uint8 FAILED=3 #on_failure +uint8 SUCCEEDED=4 #on_success uint8 UNDEFINED=5 # Related event @@ -28,6 +28,7 @@ uint8 INFO=0 uint8 DEBUG=1 uint8 ERROR=2 uint8 ERROR_ELEMENT=3 +uint8 DEFINE_EVENT=4 # Related event type uint8 type diff --git a/capabilities2_server/include/capabilities2_server/capabilities_server.hpp b/capabilities2_server/include/capabilities2_server/capabilities_server.hpp index f177673..af596cd 100644 --- a/capabilities2_server/include/capabilities2_server/capabilities_server.hpp +++ b/capabilities2_server/include/capabilities2_server/capabilities_server.hpp @@ -324,18 +324,26 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI event_options.on_started.provider = req->target_on_start.provider; event_options.on_started.parameters = req->target_on_start.parameters; + event_->define_event(req->source.capability, req->source.provider, req->target_on_start.capability, req->target_on_start.provider, capabilities2_msgs::msg::CapabilityEvent::STARTED); + event_options.on_failure.interface = req->target_on_failure.capability; event_options.on_failure.provider = req->target_on_failure.provider; event_options.on_failure.parameters = req->target_on_failure.parameters; + event_->define_event(req->source.capability, req->source.provider, req->target_on_failure.capability, req->target_on_failure.provider, capabilities2_msgs::msg::CapabilityEvent::FAILED); + event_options.on_success.interface = req->target_on_success.capability; event_options.on_success.provider = req->target_on_success.provider; event_options.on_success.parameters = req->target_on_success.parameters; + event_->define_event(req->source.capability, req->source.provider, req->target_on_success.capability, req->target_on_success.provider, capabilities2_msgs::msg::CapabilityEvent::SUCCEEDED); + event_options.on_stopped.interface = req->target_on_stop.capability; event_options.on_stopped.provider = req->target_on_stop.provider; event_options.on_stopped.parameters = req->target_on_stop.parameters; + event_->define_event(req->source.capability, req->source.provider, req->target_on_stop.capability, req->target_on_stop.provider, capabilities2_msgs::msg::CapabilityEvent::STOPPED); + // setup triggers between parameters set_triggers(req->source.capability, event_options); diff --git a/capabilities2_server/include/capabilities2_server/runner_cache.hpp b/capabilities2_server/include/capabilities2_server/runner_cache.hpp index 1d48bf7..94ac43e 100644 --- a/capabilities2_server/include/capabilities2_server/runner_cache.hpp +++ b/capabilities2_server/include/capabilities2_server/runner_cache.hpp @@ -121,11 +121,6 @@ class RunnerCache int event_count = runner_cache_[capability]->attach_events( event_options, std::bind(&capabilities2_server::RunnerCache::trigger_runner, this, std::placeholders::_1, std::placeholders::_2)); - - event_->info( - "Configured triggers for capability " + capability + ": \n\tStarted: " + event_options.on_started.interface + - " \n\tFailure: " + event_options.on_failure.interface + " \n\tSuccess: " + event_options.on_success.interface + - "\n\tStopped: " + event_options.on_stopped.interface); } /** From 9226e6f39d3810aae8862528125462034552a110 Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Thu, 15 May 2025 15:46:07 +1000 Subject: [PATCH 115/133] updated maintainer information --- capabilities2/package.xml | 1 + capabilities2_events/package.xml | 7 ++++++- capabilities2_msgs/package.xml | 1 + capabilities2_runner/package.xml | 1 + capabilities2_runner_audio/package.xml | 6 +++++- capabilities2_runner_capabilities/package.xml | 8 +++++++- capabilities2_runner_fabric/package.xml | 8 +++++++- capabilities2_runner_nav2/package.xml | 7 ++++++- capabilities2_runner_prompt/package.xml | 7 ++++++- capabilities2_server/package.xml | 1 + capabilities2_utils/package.xml | 7 ++++++- 11 files changed, 47 insertions(+), 7 deletions(-) diff --git a/capabilities2/package.xml b/capabilities2/package.xml index 691851b..a012c4d 100644 --- a/capabilities2/package.xml +++ b/capabilities2/package.xml @@ -8,6 +8,7 @@ Michael Pritchard mik-p + Kalana Ratnayake Michael Pritchard diff --git a/capabilities2_events/package.xml b/capabilities2_events/package.xml index c3ff556..fb97ed2 100644 --- a/capabilities2_events/package.xml +++ b/capabilities2_events/package.xml @@ -4,7 +4,12 @@ capabilities2_events 0.0.0 TODO: Package description - kalana + + Kalana Ratnayake + Kalana Ratnayake + + Kalana Ratnayake + TODO: License declaration ament_cmake diff --git a/capabilities2_msgs/package.xml b/capabilities2_msgs/package.xml index 3ac0ada..9130417 100644 --- a/capabilities2_msgs/package.xml +++ b/capabilities2_msgs/package.xml @@ -8,6 +8,7 @@ Michael Pritchard mik-p + Kalana Ratnayake Michael Pritchard diff --git a/capabilities2_runner/package.xml b/capabilities2_runner/package.xml index 69f7e1c..b87f01e 100644 --- a/capabilities2_runner/package.xml +++ b/capabilities2_runner/package.xml @@ -8,6 +8,7 @@ Michael Pritchard mik-p + Kalana Ratnayake Michael Pritchard diff --git a/capabilities2_runner_audio/package.xml b/capabilities2_runner_audio/package.xml index 128348b..b226bda 100644 --- a/capabilities2_runner_audio/package.xml +++ b/capabilities2_runner_audio/package.xml @@ -4,7 +4,11 @@ capabilities2_runner_audio 0.0.0 TODO: Package description - kalana + + Kalana Ratnayake + Kalana Ratnayake + + Kalana Ratnayake MIT diff --git a/capabilities2_runner_capabilities/package.xml b/capabilities2_runner_capabilities/package.xml index c383eda..8f1afe4 100644 --- a/capabilities2_runner_capabilities/package.xml +++ b/capabilities2_runner_capabilities/package.xml @@ -4,7 +4,13 @@ capabilities2_runner_capabilities 0.0.0 TODO: Package description - kalana + + + Kalana Ratnayake + Kalana Ratnayake + + Kalana Ratnayake + TODO: License declaration ament_cmake diff --git a/capabilities2_runner_fabric/package.xml b/capabilities2_runner_fabric/package.xml index 3be6c38..c94f16b 100644 --- a/capabilities2_runner_fabric/package.xml +++ b/capabilities2_runner_fabric/package.xml @@ -4,7 +4,13 @@ capabilities2_runner_fabric 0.0.0 TODO: Package description - kalana + + + Kalana Ratnayake + Kalana Ratnayake + + Kalana Ratnayake + TODO: License declaration ament_cmake diff --git a/capabilities2_runner_nav2/package.xml b/capabilities2_runner_nav2/package.xml index cb8ff67..110edc3 100644 --- a/capabilities2_runner_nav2/package.xml +++ b/capabilities2_runner_nav2/package.xml @@ -3,7 +3,12 @@ capabilities2_runner_nav2 0.0.0 TODO: Package description - kalana + + Kalana Ratnayake + Kalana Ratnayake + + Kalana Ratnayake + MIT ament_cmake diff --git a/capabilities2_runner_prompt/package.xml b/capabilities2_runner_prompt/package.xml index f1d245b..4e0301b 100644 --- a/capabilities2_runner_prompt/package.xml +++ b/capabilities2_runner_prompt/package.xml @@ -4,7 +4,12 @@ capabilities2_runner_prompt 0.0.0 TODO: Package description - kalana + + Kalana Ratnayake + Kalana Ratnayake + + Kalana Ratnayake + TODO: License declaration ament_cmake diff --git a/capabilities2_server/package.xml b/capabilities2_server/package.xml index bbe4230..226ae45 100644 --- a/capabilities2_server/package.xml +++ b/capabilities2_server/package.xml @@ -8,6 +8,7 @@ Michael Pritchard mik-p + Kalana Ratnayake Michael Pritchard diff --git a/capabilities2_utils/package.xml b/capabilities2_utils/package.xml index de33cec..f6296bd 100644 --- a/capabilities2_utils/package.xml +++ b/capabilities2_utils/package.xml @@ -4,7 +4,12 @@ capabilities2_utils 0.0.0 TODO: Package description - kalana + + Kalana Ratnayake + Kalana Ratnayake + + Kalana Ratnayake + TODO: License declaration ament_cmake From adaee64c95f3b7c9d0e197765ad365d356fa0461 Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Sat, 21 Jun 2025 05:03:09 +1000 Subject: [PATCH 116/133] extended capability event functionality to support graph visualization --- .../capabilities2_events/event_client.hpp | 69 +++++---------- .../capabilities2_events/event_listener.hpp | 85 +++++++++++++++---- capabilities2_msgs/msg/CapabilityEvent.msg | 3 +- .../capabilities2_runner/action_runner.hpp | 12 +-- .../capabilities2_runner/runner_base.hpp | 82 +++++++++++------- .../capabilities2_runner/service_runner.hpp | 14 ++- .../capabilities2_runner/topic_runner.hpp | 13 ++- .../occupancygrid_runner.hpp | 2 +- .../robotpose_runner.hpp | 18 ++-- .../capabilities_server.hpp | 12 ++- 10 files changed, 174 insertions(+), 136 deletions(-) diff --git a/capabilities2_events/include/capabilities2_events/event_client.hpp b/capabilities2_events/include/capabilities2_events/event_client.hpp index e87a433..4f13912 100644 --- a/capabilities2_events/include/capabilities2_events/event_client.hpp +++ b/capabilities2_events/include/capabilities2_events/event_client.hpp @@ -25,6 +25,16 @@ class EventClient event_publisher_ = node_->create_publisher(topic_name, 10); } + /** + * @brief publishes status information to the given topic + * + * @param message Message to be published + */ + void publish(const Event& message) + { + event_publisher_->publish(message); + } + /** * @brief publishes status information to the given topic as info * @@ -47,17 +57,7 @@ class EventClient message.content = text; message.pid = -1; - event_publisher_->publish(message); - } - - /** - * @brief publishes status information to the given topic as info - * - * @param message Message to be published - */ - void info(const Event& message) - { - event_publisher_->publish(message); + publish(message); } /** @@ -82,17 +82,7 @@ class EventClient message.content = text; message.pid = -1; - event_publisher_->publish(message); - } - - /** - * @brief publishes status information to the given topic as debug - * - * @param message Message to be published - */ - void debug(const Event& message) - { - event_publisher_->publish(message); + publish(message); } /** @@ -116,17 +106,7 @@ class EventClient message.content = text; message.pid = -1; - event_publisher_->publish(message); - } - - /** - * @brief publishes status information to the given topic as error - * - * @param message Message to be published - */ - void error(const Event& message) - { - event_publisher_->publish(message); + publish(message); } /** @@ -150,27 +130,16 @@ class EventClient message.content = element; message.pid = -1; - event_publisher_->publish(message); - } - - /** - * @brief publishes status information to the given topic as error with - * element infromation included - * - * @param message Message to be published - */ - void error_element(const Event& message) - { - event_publisher_->publish(message); + publish(message); } /** - * @brief publishes element information to the given topic as error + * @brief publishes element information to the given topic as event * * @param element element information to be published */ - void define_event(const std::string& src_capability, const std::string& src_provider, const std::string& tgt_capability, - const std::string& tgt_provider, uint8_t event = Event::UNDEFINED) + void runner_define(const std::string& src_capability, const std::string& src_provider, const std::string& tgt_capability, + const std::string& tgt_provider, uint8_t event = Event::UNDEFINED) { auto message = Event(); @@ -182,11 +151,11 @@ class EventClient message.target.provider = tgt_provider; message.thread_id = -1; message.event = event; - message.type = Event::DEFINE_EVENT; + message.type = Event::RUNNER_DEFINE; message.content = ""; message.pid = -1; - event_publisher_->publish(message); + publish(message); } protected: diff --git a/capabilities2_events/include/capabilities2_events/event_listener.hpp b/capabilities2_events/include/capabilities2_events/event_listener.hpp index fadb4c0..b5a1781 100644 --- a/capabilities2_events/include/capabilities2_events/event_listener.hpp +++ b/capabilities2_events/include/capabilities2_events/event_listener.hpp @@ -18,54 +18,105 @@ class CapabilitiesEventListener : public rclcpp::Node } private: + /** + * @brief Constructs a message from the event and logs it + * + * @param msg + */ void topic_callback(const Event& msg) const { std::string text; + // Construct the text based on the event type and content + + // if the event contains and errornous element, we log it as an error if (msg.type == Event::ERROR_ELEMENT) { text = "[" + msg.origin_node + "]" + "[" + msg.source.capability + "/" + std::to_string(msg.thread_id) + "] " + msg.content; } - else if (msg.type == Event::DEFINE_EVENT and msg.event == Event::STARTED) + // If the event is at runner definition and logs on start event + else if (msg.type == Event::RUNNER_DEFINE and msg.event == Event::STARTED) { text = "[" + msg.origin_node + "]" + "[" + msg.source.capability + "] will trigger [" + msg.target.capability + "] on start"; } - else if (msg.type == Event::DEFINE_EVENT and msg.event == Event::STOPPED) + // If the event is at runner definition and logs on stop event + else if (msg.type == Event::RUNNER_DEFINE and msg.event == Event::STOPPED) { text = "[" + msg.origin_node + "]" + "[" + msg.source.capability + "] will trigger [" + msg.target.capability + "] on stop"; } - else if (msg.type == Event::DEFINE_EVENT and msg.event == Event::FAILED) + // If the event is at runner definition and logs on failure event + else if (msg.type == Event::RUNNER_DEFINE and msg.event == Event::FAILED) { text = "[" + msg.origin_node + "]" + "[" + msg.source.capability + "] will trigger [" + msg.target.capability + "] on failure"; } - else if (msg.type == Event::DEFINE_EVENT and msg.event == Event::SUCCEEDED) + // If the event is at runner definition and logs on success event + else if (msg.type == Event::RUNNER_DEFINE and msg.event == Event::SUCCEEDED) { text = "[" + msg.origin_node + "]" + "[" + msg.source.capability + "] will trigger [" + msg.target.capability + "] on success"; } - else if (msg.thread_id >= 0 and msg.target.capability == "" and msg.source.capability == "") + // If the event is at runner execution and logs on start event from main thread + else if (msg.type == Event::RUNNER_EVENT and msg.event == Event::STARTED and msg.thread_id < 0) { - text = "[" + msg.origin_node + "]" + "[" + std::to_string(msg.thread_id) + "] " + msg.content; + text = "[" + msg.origin_node + "]" + "[" + msg.source.capability + "] triggering [" + msg.target.capability + "] on start"; + } + // If the event is at runner execution and logs on start event from worker thread + else if (msg.type == Event::RUNNER_EVENT and msg.event == Event::STARTED and msg.thread_id >= 0) + { + text = "[" + msg.origin_node + "]" + "[" + msg.source.capability + "/" + std::to_string(msg.thread_id) + "] triggering " + + msg.target.capability + "] on start"; + } + // If the event is at runner execution and logs on stop event from main thread + else if (msg.type == Event::RUNNER_EVENT and msg.event == Event::STOPPED and msg.thread_id < 0) + { + text = "[" + msg.origin_node + "]" + "[" + msg.source.capability + "] triggering [" + msg.target.capability + "] on stop"; + } + // If the event is at runner execution and logs on stop event from worker thread + else if (msg.type == Event::RUNNER_EVENT and msg.event == Event::STOPPED and msg.thread_id >= 0) + { + text = "[" + msg.origin_node + "]" + "[" + msg.source.capability + "/" + std::to_string(msg.thread_id) + "] triggering " + + msg.target.capability + "] on stop"; + } + // If the event is at runner execution and logs on failure event from main thread + else if (msg.type == Event::RUNNER_EVENT and msg.event == Event::FAILED and msg.thread_id < 0) + { + text = "[" + msg.origin_node + "]" + "[" + msg.source.capability + "] triggering [" + msg.target.capability + "] on failure"; + } + // If the event is at runner execution and logs on failure event from worker thread + else if (msg.type == Event::RUNNER_EVENT and msg.event == Event::FAILED and msg.thread_id >= 0) + { + text = "[" + msg.origin_node + "]" + "[" + msg.source.capability + "/" + std::to_string(msg.thread_id) + "] triggering " + + msg.target.capability + "] on failure"; } + // If the event is at runner execution and logs on success event from main thread + else if (msg.type == Event::RUNNER_EVENT and msg.event == Event::SUCCEEDED and msg.thread_id < 0) + { + text = "[" + msg.origin_node + "]" + "[" + msg.source.capability + "] triggering [" + msg.target.capability + "] on success"; + } + // If the event is at runner execution and logs on success event from worker thread + else if (msg.type == Event::RUNNER_EVENT and msg.event == Event::SUCCEEDED and msg.thread_id >= 0) + { + text = "[" + msg.origin_node + "]" + "[" + msg.source.capability + "/" + std::to_string(msg.thread_id) + "] triggering " + + msg.target.capability + "] on success"; + } + // no capabilities means running on main node, thread id is -1 means on main thread else if (msg.thread_id < 0 and msg.target.capability == "" and msg.source.capability == "") { text = "[" + msg.origin_node + "] " + msg.content; } - else if (msg.thread_id >= 0 and msg.target.capability == "" and msg.source.capability != "") + // no capabilities means running on main node, thread id is >0 means on worker thread. + else if (msg.thread_id >= 0 and msg.target.capability == "" and msg.source.capability == "") { - text = "[" + msg.origin_node + "]" + "[" + msg.source.capability + "/" + std::to_string(msg.thread_id) + "] " + msg.content; + text = "[" + msg.origin_node + "]" + "[" + std::to_string(msg.thread_id) + "] " + msg.content; } + // source capability is set, target capability is not set means msg is from within a capability, thread id is -1 means on main thread else if (msg.thread_id < 0 and msg.target.capability == "" and msg.source.capability != "") { text = "[" + msg.origin_node + "]" + "[" + msg.source.capability + "] " + msg.content; } - else if (msg.thread_id >= 0 and msg.target.capability != "") - { - text = "[" + msg.origin_node + "]" + "[" + msg.source.capability + "/" + std::to_string(msg.thread_id) + "] triggering " + - msg.target.capability + " " + msg.content; - } - else if (msg.thread_id < 0 and msg.target.capability != "") + // source capability is set, target capability is not set means msg is from within a capability, thread id is >0 means on worker thread. + else if (msg.thread_id >= 0 and msg.target.capability == "" and msg.source.capability != "") { - text = "[" + msg.origin_node + "]" + "[" + msg.source.capability + "] triggering " + msg.target.capability + " " + msg.content; + text = "[" + msg.origin_node + "]" + "[" + msg.source.capability + "/" + std::to_string(msg.thread_id) + "] " + msg.content; } if (msg.type == Event::ERROR) @@ -76,7 +127,9 @@ class CapabilitiesEventListener : public rclcpp::Node RCLCPP_DEBUG(get_logger(), text.c_str()); else if (msg.type == Event::INFO) RCLCPP_INFO(get_logger(), text.c_str()); - else if (msg.type == Event::DEFINE_EVENT) + else if (msg.type == Event::RUNNER_DEFINE) + RCLCPP_INFO(get_logger(), text.c_str()); + else if (msg.type == Event::RUNNER_EVENT) RCLCPP_INFO(get_logger(), text.c_str()); else RCLCPP_INFO(get_logger(), text.c_str()); diff --git a/capabilities2_msgs/msg/CapabilityEvent.msg b/capabilities2_msgs/msg/CapabilityEvent.msg index e5adef1..3c8d449 100644 --- a/capabilities2_msgs/msg/CapabilityEvent.msg +++ b/capabilities2_msgs/msg/CapabilityEvent.msg @@ -28,7 +28,8 @@ uint8 INFO=0 uint8 DEBUG=1 uint8 ERROR=2 uint8 ERROR_ELEMENT=3 -uint8 DEFINE_EVENT=4 +uint8 RUNNER_DEFINE=4 +uint8 RUNNER_EVENT=5 # Related event type uint8 type diff --git a/capabilities2_runner/include/capabilities2_runner/action_runner.hpp b/capabilities2_runner/include/capabilities2_runner/action_runner.hpp index 2674f15..f4c88c8 100644 --- a/capabilities2_runner/include/capabilities2_runner/action_runner.hpp +++ b/capabilities2_runner/include/capabilities2_runner/action_runner.hpp @@ -93,8 +93,8 @@ class ActionRunner : public RunnerBase // Trigger on_stopped event if defined if (events[execute_id].on_stopped.interface != "") { - info_("on_stopped", -1, EventType::STOPPED, events[execute_id].on_stopped.interface, - events[execute_id].on_stopped.provider); + event_(EventType::STOPPED, -1, events[execute_id].on_stopped.interface, + events[execute_id].on_stopped.provider); triggerFunction_(events[execute_id].on_stopped.interface, update_on_stopped(events[execute_id].on_stopped.parameters)); } @@ -153,8 +153,8 @@ class ActionRunner : public RunnerBase // trigger the events related to on_started state if (events[execute_id].on_started.interface != "") { - info_("on_started", id, EventType::STARTED, events[execute_id].on_started.interface, - events[execute_id].on_started.provider); + event_(EventType::STARTED, id, events[execute_id].on_started.interface, + events[execute_id].on_started.provider); triggerFunction_(events[execute_id].on_started.interface, update_on_started(events[execute_id].on_started.parameters)); } @@ -189,7 +189,7 @@ class ActionRunner : public RunnerBase // trigger the events related to on_success state if (events[execute_id].on_success.interface != "") { - info_("on_success", id, EventType::SUCCEEDED, events[execute_id].on_success.interface, + event_(EventType::SUCCEEDED, id, events[execute_id].on_success.interface, events[execute_id].on_success.provider); triggerFunction_(events[execute_id].on_success.interface, update_on_success(events[execute_id].on_success.parameters)); @@ -202,7 +202,7 @@ class ActionRunner : public RunnerBase // trigger the events related to on_failure state if (events[execute_id].on_failure.interface != "") { - info_("on_failure", id, EventType::FAILED, events[execute_id].on_failure.interface, + event_(EventType::FAILED, id, events[execute_id].on_failure.interface, events[execute_id].on_failure.provider); triggerFunction_(events[execute_id].on_failure.interface, update_on_failure(events[execute_id].on_failure.parameters)); diff --git a/capabilities2_runner/include/capabilities2_runner/runner_base.hpp b/capabilities2_runner/include/capabilities2_runner/runner_base.hpp index f9e702f..2d30ee6 100644 --- a/capabilities2_runner/include/capabilities2_runner/runner_base.hpp +++ b/capabilities2_runner/include/capabilities2_runner/runner_base.hpp @@ -131,7 +131,7 @@ class RunnerBase execute_id = -1; thread_id = 0; - event_ = std::make_shared(node_, "runner", "/events"); + event_client_ = std::make_shared(node_, "runner", "/events"); } /** @@ -447,8 +447,7 @@ class RunnerBase } protected: - void info_(const std::string text, int thread_id = -1, EventType event = EventType::IDLE, - const std::string& target_capability = "", const std::string& target_provider = "") + void info_(const std::string text, int thread_id = -1) { auto message = Event(); @@ -456,36 +455,15 @@ class RunnerBase message.origin_node = "runners"; message.source.capability = run_config_.interface; message.source.provider = run_config_.provider; - message.target.capability = target_capability; - message.target.provider = target_provider; + message.target.capability = ""; + message.target.provider = ""; message.thread_id = thread_id; message.type = Event::INFO; message.content = text; message.pid = -1; + message.event = Event::UNDEFINED; - switch (event) - { - case EventType::IDLE: - message.event = Event::IDLE; - break; - case EventType::STARTED: - message.event = Event::STARTED; - break; - case EventType::STOPPED: - message.event = Event::STOPPED; - break; - case EventType::FAILED: - message.event = Event::FAILED; - break; - case EventType::SUCCEEDED: - message.event = Event::SUCCEEDED; - break; - default: - message.event = Event::UNDEFINED; - break; - } - - event_->info(message); + event_client_->publish(message); } void error_(const std::string text, int thread_id = -1) @@ -504,7 +482,7 @@ class RunnerBase message.pid = -1; message.event = Event::UNDEFINED; - event_->error(message); + event_client_->publish(message); } void output_(const std::string text, const std::string element, int thread_id = -1) @@ -518,12 +496,52 @@ class RunnerBase message.target.capability = ""; message.target.provider = ""; message.thread_id = thread_id; - message.type = Event::ERROR_ELEMENT; + message.type = Event::INFO; message.content = text + " : " + element; message.pid = -1; message.event = Event::UNDEFINED; - event_->error_element(message); + event_client_->publish(message); + } + + void event_(EventType event = EventType::IDLE, int thread_id = -1, const std::string& target_capability = "", + const std::string& target_provider = "") + { + auto message = Event(); + + message.header.stamp = node_->now(); + message.origin_node = "runners"; + message.source.capability = run_config_.interface; + message.source.provider = run_config_.provider; + message.target.capability = target_capability; + message.target.provider = target_provider; + message.thread_id = thread_id; + message.type = Event::RUNNER_EVENT; + message.pid = -1; + + switch (event) + { + case EventType::IDLE: + message.event = Event::IDLE; + break; + case EventType::STARTED: + message.event = Event::STARTED; + break; + case EventType::STOPPED: + message.event = Event::STOPPED; + break; + case EventType::FAILED: + message.event = Event::FAILED; + break; + case EventType::SUCCEEDED: + message.event = Event::SUCCEEDED; + break; + default: + message.event = Event::UNDEFINED; + break; + } + + event_client_->publish(message); } /** @@ -599,7 +617,7 @@ class RunnerBase /** * @brief client for publishing events */ - std::shared_ptr event_; + std::shared_ptr event_client_; }; } // namespace capabilities2_runner diff --git a/capabilities2_runner/include/capabilities2_runner/service_runner.hpp b/capabilities2_runner/include/capabilities2_runner/service_runner.hpp index 732255c..d0017ef 100644 --- a/capabilities2_runner/include/capabilities2_runner/service_runner.hpp +++ b/capabilities2_runner/include/capabilities2_runner/service_runner.hpp @@ -84,8 +84,8 @@ class ServiceRunner : public RunnerBase // trigger the events related to on_failure state if (events[execute_id].on_failure.interface != "") { - info_("on_failure", id, EventType::FAILED, events[execute_id].on_failure.interface, - events[execute_id].on_failure.provider); + event_(EventType::FAILED, id, events[execute_id].on_failure.interface, + events[execute_id].on_failure.provider); triggerFunction_(events[execute_id].on_failure.interface, update_on_failure(events[execute_id].on_failure.parameters)); } @@ -100,8 +100,8 @@ class ServiceRunner : public RunnerBase // trigger the events related to on_success state if (events[execute_id].on_success.interface != "") { - info_("on_success", id, EventType::SUCCEEDED, events[execute_id].on_success.interface, - events[execute_id].on_success.provider); + event_(EventType::SUCCEEDED, id, events[execute_id].on_success.interface, + events[execute_id].on_success.provider); triggerFunction_(events[execute_id].on_success.interface, update_on_success(events[execute_id].on_success.parameters)); } @@ -114,8 +114,7 @@ class ServiceRunner : public RunnerBase // trigger the events related to on_started state if (events[execute_id].on_started.interface != "") { - info_("on_started", id, EventType::STARTED, events[execute_id].on_started.interface, - events[execute_id].on_started.provider); + event_(EventType::STARTED, id, events[execute_id].on_started.interface, events[execute_id].on_started.provider); triggerFunction_(events[execute_id].on_started.interface, update_on_started(events[execute_id].on_started.parameters)); } @@ -146,8 +145,7 @@ class ServiceRunner : public RunnerBase // Trigger on_stopped event if defined if (events[execute_id].on_stopped.interface != "") { - info_("on_stopped", -1, EventType::STOPPED, events[execute_id].on_stopped.interface, - events[execute_id].on_stopped.provider); + event_(EventType::STOPPED, -1, events[execute_id].on_stopped.interface, events[execute_id].on_stopped.provider); triggerFunction_(events[execute_id].on_stopped.interface, update_on_stopped(events[execute_id].on_stopped.parameters)); } diff --git a/capabilities2_runner/include/capabilities2_runner/topic_runner.hpp b/capabilities2_runner/include/capabilities2_runner/topic_runner.hpp index 06b3875..f2a6893 100644 --- a/capabilities2_runner/include/capabilities2_runner/topic_runner.hpp +++ b/capabilities2_runner/include/capabilities2_runner/topic_runner.hpp @@ -60,8 +60,7 @@ class TopicRunner : public RunnerBase // trigger the events related to on_started state if (events[execute_id].on_started.interface != "") { - info_("on_started", id, EventType::STARTED, events[execute_id].on_started.interface, - events[execute_id].on_started.provider); + event_(EventType::STARTED, id, events[execute_id].on_started.interface, events[execute_id].on_started.provider); triggerFunction_(events[execute_id].on_started.interface, update_on_started(events[execute_id].on_started.parameters)); } @@ -80,8 +79,8 @@ class TopicRunner : public RunnerBase // trigger the events related to on_success state if (events[execute_id].on_success.interface != "") { - info_("on_success", id, EventType::SUCCEEDED, events[execute_id].on_success.interface, - events[execute_id].on_success.provider); + event_(EventType::SUCCEEDED, id, events[execute_id].on_success.interface, + events[execute_id].on_success.provider); triggerFunction_(events[execute_id].on_success.interface, update_on_success(events[execute_id].on_success.parameters)); } @@ -93,8 +92,7 @@ class TopicRunner : public RunnerBase // trigger the events related to on_failure state if (events[execute_id].on_failure.interface != "") { - info_("on_failure", id, EventType::FAILED, events[execute_id].on_failure.interface, - events[execute_id].on_failure.provider); + event_(EventType::FAILED, id, events[execute_id].on_failure.interface, events[execute_id].on_failure.provider); triggerFunction_(events[execute_id].on_failure.interface, update_on_failure(events[execute_id].on_failure.parameters)); } @@ -124,8 +122,7 @@ class TopicRunner : public RunnerBase // Trigger on_stopped event if defined if (events[execute_id].on_stopped.interface != "") { - info_("on_stopped", -1, EventType::STOPPED, events[execute_id].on_stopped.interface, - events[execute_id].on_stopped.provider); + event_(EventType::STOPPED, -1, events[execute_id].on_stopped.interface, events[execute_id].on_stopped.provider); triggerFunction_(events[execute_id].on_stopped.interface, update_on_stopped(events[execute_id].on_stopped.parameters)); } diff --git a/capabilities2_runner_nav2/include/capabilities2_runner_nav2/occupancygrid_runner.hpp b/capabilities2_runner_nav2/include/capabilities2_runner_nav2/occupancygrid_runner.hpp index 7856b7d..f979f28 100644 --- a/capabilities2_runner_nav2/include/capabilities2_runner_nav2/occupancygrid_runner.hpp +++ b/capabilities2_runner_nav2/include/capabilities2_runner_nav2/occupancygrid_runner.hpp @@ -124,7 +124,7 @@ class OccupancyGridRunner : public TopicRunner // Return the updated parameters element with OccupancyGrid added std::string result = convert_to_string(element); - output_("on_success trigger parameter", result); + // output_("on_success trigger parameter", result); return result; }; diff --git a/capabilities2_runner_nav2/include/capabilities2_runner_nav2/robotpose_runner.hpp b/capabilities2_runner_nav2/include/capabilities2_runner_nav2/robotpose_runner.hpp index f42efb5..5ce0429 100644 --- a/capabilities2_runner_nav2/include/capabilities2_runner_nav2/robotpose_runner.hpp +++ b/capabilities2_runner_nav2/include/capabilities2_runner_nav2/robotpose_runner.hpp @@ -64,8 +64,7 @@ class RobotPoseRunner : public RunnerBase // trigger the events related to on_started state if (events[execute_id].on_started.interface != "") { - info_("on_started", id, EventType::STARTED, events[execute_id].on_started.interface, - events[execute_id].on_started.provider); + event_(EventType::STARTED, id, events[execute_id].on_started.interface, events[execute_id].on_started.provider); triggerFunction_(events[execute_id].on_started.interface, update_on_started(events[execute_id].on_started.parameters)); } @@ -90,8 +89,8 @@ class RobotPoseRunner : public RunnerBase // trigger the events related to on_success state if (events[execute_id].on_success.interface != "") { - info_("on_success", id, EventType::SUCCEEDED, events[execute_id].on_success.interface, - events[execute_id].on_success.provider); + event_(EventType::SUCCEEDED, id, events[execute_id].on_success.interface, + events[execute_id].on_success.provider); triggerFunction_(events[execute_id].on_success.interface, update_on_success(events[execute_id].on_success.parameters)); } @@ -112,8 +111,8 @@ class RobotPoseRunner : public RunnerBase // trigger the events related to on_success state if (events[execute_id].on_success.interface != "") { - info_("on_success", id, EventType::SUCCEEDED, events[execute_id].on_success.interface, - events[execute_id].on_success.provider); + event_(EventType::SUCCEEDED, id, events[execute_id].on_success.interface, + events[execute_id].on_success.provider); triggerFunction_(events[execute_id].on_success.interface, update_on_success(events[execute_id].on_success.parameters)); } @@ -127,8 +126,7 @@ class RobotPoseRunner : public RunnerBase // trigger the events related to on_failure state if (events[execute_id].on_failure.interface != "") { - info_("on_failure", id, EventType::FAILED, events[execute_id].on_failure.interface, - events[execute_id].on_failure.provider); + event_(EventType::FAILED, id, events[execute_id].on_failure.interface, events[execute_id].on_failure.provider); triggerFunction_(events[execute_id].on_failure.interface, update_on_failure(events[execute_id].on_failure.parameters)); } @@ -158,7 +156,7 @@ class RobotPoseRunner : public RunnerBase // Trigger on_stopped event if defined if (events[execute_id].on_stopped.interface != "") { - info_("on_stopped", -1, EventType::STOPPED, events[execute_id].on_stopped.interface, + event_(EventType::STOPPED, -1, events[execute_id].on_stopped.interface, events[execute_id].on_stopped.provider); triggerFunction_(events[execute_id].on_stopped.interface, update_on_stopped(events[execute_id].on_stopped.parameters)); @@ -206,7 +204,7 @@ class RobotPoseRunner : public RunnerBase // Return the updated parameters element with Pose added as string std::string result = convert_to_string(element); - output_("on_success trigger parameter", result); + // output_("on_success trigger parameter", result); return result; }; diff --git a/capabilities2_server/include/capabilities2_server/capabilities_server.hpp b/capabilities2_server/include/capabilities2_server/capabilities_server.hpp index af596cd..0df4fb5 100644 --- a/capabilities2_server/include/capabilities2_server/capabilities_server.hpp +++ b/capabilities2_server/include/capabilities2_server/capabilities_server.hpp @@ -324,25 +324,29 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI event_options.on_started.provider = req->target_on_start.provider; event_options.on_started.parameters = req->target_on_start.parameters; - event_->define_event(req->source.capability, req->source.provider, req->target_on_start.capability, req->target_on_start.provider, capabilities2_msgs::msg::CapabilityEvent::STARTED); + event_->runner_define(req->source.capability, req->source.provider, req->target_on_start.capability, + req->target_on_start.provider, capabilities2_msgs::msg::CapabilityEvent::STARTED); event_options.on_failure.interface = req->target_on_failure.capability; event_options.on_failure.provider = req->target_on_failure.provider; event_options.on_failure.parameters = req->target_on_failure.parameters; - event_->define_event(req->source.capability, req->source.provider, req->target_on_failure.capability, req->target_on_failure.provider, capabilities2_msgs::msg::CapabilityEvent::FAILED); + event_->runner_define(req->source.capability, req->source.provider, req->target_on_failure.capability, + req->target_on_failure.provider, capabilities2_msgs::msg::CapabilityEvent::FAILED); event_options.on_success.interface = req->target_on_success.capability; event_options.on_success.provider = req->target_on_success.provider; event_options.on_success.parameters = req->target_on_success.parameters; - event_->define_event(req->source.capability, req->source.provider, req->target_on_success.capability, req->target_on_success.provider, capabilities2_msgs::msg::CapabilityEvent::SUCCEEDED); + event_->runner_define(req->source.capability, req->source.provider, req->target_on_success.capability, + req->target_on_success.provider, capabilities2_msgs::msg::CapabilityEvent::SUCCEEDED); event_options.on_stopped.interface = req->target_on_stop.capability; event_options.on_stopped.provider = req->target_on_stop.provider; event_options.on_stopped.parameters = req->target_on_stop.parameters; - event_->define_event(req->source.capability, req->source.provider, req->target_on_stop.capability, req->target_on_stop.provider, capabilities2_msgs::msg::CapabilityEvent::STOPPED); + event_->runner_define(req->source.capability, req->source.provider, req->target_on_stop.capability, + req->target_on_stop.provider, capabilities2_msgs::msg::CapabilityEvent::STOPPED); // setup triggers between parameters set_triggers(req->source.capability, event_options); From 6d48948e2fc40bb5458622dc979ec7aafaaa3fe4 Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Fri, 4 Jul 2025 01:58:04 +1000 Subject: [PATCH 117/133] moved events into a new independent package for better integration --- capabilities2_events/.clang-format | 64 ------- capabilities2_events/CMakeLists.txt | 53 ------ .../capabilities2_events/event_client.hpp | 179 ------------------ .../capabilities2_events/event_listener.hpp | 139 -------------- .../capabilities2_events/event_types.hpp | 44 ----- .../launch/foxglove.launch.py | 61 ------ .../launch/listener.launch.py | 28 --- capabilities2_events/package.xml | 28 --- .../src/event_listener_node.cpp | 17 -- .../capabilities_launch_proxy.py | 8 - capabilities2_msgs/CMakeLists.txt | 1 - capabilities2_msgs/action/Launch.action | 1 - capabilities2_msgs/msg/CapabilityEvent.msg | 41 ---- capabilities2_runner/CMakeLists.txt | 6 +- .../capabilities2_runner/runner_base.hpp | 46 ++--- capabilities2_runner/package.xml | 3 +- capabilities2_runner_audio/CMakeLists.txt | 6 +- capabilities2_runner_audio/package.xml | 3 +- capabilities2_runner_bt/CMakeLists.txt | 6 +- capabilities2_runner_bt/package.xml | 3 +- .../CMakeLists.txt | 6 +- capabilities2_runner_capabilities/package.xml | 3 +- capabilities2_runner_fabric/CMakeLists.txt | 6 +- capabilities2_runner_fabric/package.xml | 3 +- capabilities2_runner_nav2/CMakeLists.txt | 6 +- capabilities2_runner_nav2/package.xml | 3 +- capabilities2_runner_prompt/CMakeLists.txt | 6 +- capabilities2_runner_prompt/package.xml | 3 +- capabilities2_server/CMakeLists.txt | 9 +- .../capabilities2_server/capabilities_api.hpp | 9 +- .../capabilities_server.hpp | 16 +- .../capabilities2_server/runner_cache.hpp | 4 +- capabilities2_server/package.xml | 3 +- .../capabilities2_utils/bond_client.hpp | 2 +- 34 files changed, 89 insertions(+), 727 deletions(-) delete mode 100644 capabilities2_events/.clang-format delete mode 100644 capabilities2_events/CMakeLists.txt delete mode 100644 capabilities2_events/include/capabilities2_events/event_client.hpp delete mode 100644 capabilities2_events/include/capabilities2_events/event_listener.hpp delete mode 100644 capabilities2_events/include/capabilities2_events/event_types.hpp delete mode 100644 capabilities2_events/launch/foxglove.launch.py delete mode 100644 capabilities2_events/launch/listener.launch.py delete mode 100644 capabilities2_events/package.xml delete mode 100644 capabilities2_events/src/event_listener_node.cpp delete mode 100644 capabilities2_msgs/msg/CapabilityEvent.msg diff --git a/capabilities2_events/.clang-format b/capabilities2_events/.clang-format deleted file mode 100644 index 92effdd..0000000 --- a/capabilities2_events/.clang-format +++ /dev/null @@ -1,64 +0,0 @@ - -BasedOnStyle: Google -AccessModifierOffset: -2 -ConstructorInitializerIndentWidth: 2 -AlignEscapedNewlinesLeft: false -AlignTrailingComments: true -AllowAllParametersOfDeclarationOnNextLine: false -AllowShortIfStatementsOnASingleLine: false -AllowShortLoopsOnASingleLine: false -AllowShortFunctionsOnASingleLine: None -AlwaysBreakTemplateDeclarations: true -AlwaysBreakBeforeMultilineStrings: false -BreakBeforeBinaryOperators: false -BreakBeforeTernaryOperators: false -BreakConstructorInitializersBeforeComma: true -BinPackParameters: true -ColumnLimit: 150 -ConstructorInitializerAllOnOneLineOrOnePerLine: true -DerivePointerBinding: false -PointerBindsToType: true -ExperimentalAutoDetectBinPacking: false -IndentCaseLabels: true -MaxEmptyLinesToKeep: 1 -NamespaceIndentation: None -ObjCSpaceBeforeProtocolList: true -PenaltyBreakBeforeFirstCallParameter: 19 -PenaltyBreakComment: 60 -PenaltyBreakString: 1 -PenaltyBreakFirstLessLess: 1000 -PenaltyExcessCharacter: 1000 -PenaltyReturnTypeOnItsOwnLine: 90 -SpacesBeforeTrailingComments: 2 -Cpp11BracedListStyle: false -Standard: Auto -IndentWidth: 2 -TabWidth: 2 -UseTab: Never -IndentFunctionDeclarationAfterType: false -SpacesInParentheses: false -SpacesInAngles: false -SpaceInEmptyParentheses: false -SpacesInCStyleCastParentheses: false -SpaceAfterControlStatementKeyword: true -SpaceBeforeAssignmentOperators: true -ContinuationIndentWidth: 4 -SortIncludes: false -SpaceAfterCStyleCast: false - -# Configure each individual brace in BraceWrapping -BreakBeforeBraces: Custom - -# Control of individual brace wrapping cases -BraceWrapping: { - AfterClass: 'true' - AfterControlStatement: 'true' - AfterEnum : 'true' - AfterFunction : 'true' - AfterNamespace : 'true' - AfterStruct : 'true' - AfterUnion : 'true' - BeforeCatch : 'true' - BeforeElse : 'true' - IndentBraces : 'false' -} diff --git a/capabilities2_events/CMakeLists.txt b/capabilities2_events/CMakeLists.txt deleted file mode 100644 index 52b569a..0000000 --- a/capabilities2_events/CMakeLists.txt +++ /dev/null @@ -1,53 +0,0 @@ -cmake_minimum_required(VERSION 3.10) -project(capabilities2_events) - -# Default to C++17 -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) -endif() - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -find_package(ament_cmake REQUIRED) -find_package(rclcpp REQUIRED) -find_package(capabilities2_msgs REQUIRED) - -include_directories( - include -) - -############################################################################ -# node implementation that compiles as a executable -############################################################################ - -add_executable(${PROJECT_NAME}_node - src/event_listener_node.cpp -) - -# target_link_libraries(${PROJECT_NAME}_node) - -ament_target_dependencies(${PROJECT_NAME}_node - rclcpp - capabilities2_msgs -) - -install(TARGETS ${PROJECT_NAME}_node - DESTINATION lib/${PROJECT_NAME} -) - -############################################################################ -# miscellaneous -############################################################################ - -install(DIRECTORY include/ - DESTINATION include -) - -install(DIRECTORY launch - DESTINATION share/${PROJECT_NAME} -) - -ament_export_include_directories(include) -ament_package() diff --git a/capabilities2_events/include/capabilities2_events/event_client.hpp b/capabilities2_events/include/capabilities2_events/event_client.hpp deleted file mode 100644 index 4f13912..0000000 --- a/capabilities2_events/include/capabilities2_events/event_client.hpp +++ /dev/null @@ -1,179 +0,0 @@ -#pragma once -#include -#include - -/** - * @brief A class to publish events to a given topic - * - */ -class EventClient -{ -public: - using Event = capabilities2_msgs::msg::CapabilityEvent; - - /** - * @brief Construct a new Status Client object - * - * @param node Pointer to the node - * @param node_name node name to be used for status message - * @param topic_name topic name to publish the message - */ - EventClient(rclcpp::Node::SharedPtr node, const std::string& node_name, const std::string& topic_name) - { - node_ = node; - node_name_ = node_name; - event_publisher_ = node_->create_publisher(topic_name, 10); - } - - /** - * @brief publishes status information to the given topic - * - * @param message Message to be published - */ - void publish(const Event& message) - { - event_publisher_->publish(message); - } - - /** - * @brief publishes status information to the given topic as info - * - * @param text Text to be published - */ - - void info(const std::string& text, int thread_id = -1) - { - auto message = Event(); - - message.header.stamp = node_->now(); - message.origin_node = node_name_; - message.source.capability = ""; - message.source.provider = ""; - message.target.capability = ""; - message.target.provider = ""; - message.thread_id = thread_id; - message.event = Event::UNDEFINED; - message.type = Event::INFO; - message.content = text; - message.pid = -1; - - publish(message); - } - - /** - * @brief publishes status information to the given topic as debug - * - * @param text Text to be published - */ - - void debug(const std::string& text, int thread_id = -1) - { - auto message = Event(); - - message.header.stamp = node_->now(); - message.origin_node = node_name_; - message.source.capability = ""; - message.source.provider = ""; - message.target.capability = ""; - message.target.provider = ""; - message.thread_id = thread_id; - message.event = Event::UNDEFINED; - message.type = Event::DEBUG; - message.content = text; - message.pid = -1; - - publish(message); - } - - /** - * @brief publishes status information to the given topic as error - * - * @param text Text to be published - */ - void error(const std::string& text, int thread_id = -1) - { - auto message = Event(); - - message.header.stamp = node_->now(); - message.origin_node = node_name_; - message.source.capability = ""; - message.source.provider = ""; - message.target.capability = ""; - message.target.provider = ""; - message.thread_id = thread_id; - message.event = Event::UNDEFINED; - message.type = Event::ERROR; - message.content = text; - message.pid = -1; - - publish(message); - } - - /** - * @brief publishes element information to the given topic as error - * - * @param element element information to be published - */ - void error_element(const std::string& element, int thread_id = -1) - { - auto message = Event(); - - message.header.stamp = node_->now(); - message.origin_node = node_name_; - message.source.capability = ""; - message.source.provider = ""; - message.target.capability = ""; - message.target.provider = ""; - message.thread_id = thread_id; - message.event = Event::UNDEFINED; - message.type = Event::ERROR_ELEMENT; - message.content = element; - message.pid = -1; - - publish(message); - } - - /** - * @brief publishes element information to the given topic as event - * - * @param element element information to be published - */ - void runner_define(const std::string& src_capability, const std::string& src_provider, const std::string& tgt_capability, - const std::string& tgt_provider, uint8_t event = Event::UNDEFINED) - { - auto message = Event(); - - message.header.stamp = node_->now(); - message.origin_node = node_name_; - message.source.capability = src_capability; - message.source.provider = src_provider; - message.target.capability = tgt_capability; - message.target.provider = tgt_provider; - message.thread_id = -1; - message.event = event; - message.type = Event::RUNNER_DEFINE; - message.content = ""; - message.pid = -1; - - publish(message); - } - -protected: - /** - * @brief Node pointer to access logging interface - * - */ - rclcpp::Node::SharedPtr node_; - - /** - * @brief publisher to publish execution status - * - */ - rclcpp::Publisher::SharedPtr event_publisher_; - - /** - * @brief Node name - * - */ - std::string node_name_; -}; \ No newline at end of file diff --git a/capabilities2_events/include/capabilities2_events/event_listener.hpp b/capabilities2_events/include/capabilities2_events/event_listener.hpp deleted file mode 100644 index b5a1781..0000000 --- a/capabilities2_events/include/capabilities2_events/event_listener.hpp +++ /dev/null @@ -1,139 +0,0 @@ -#include - -#include "rclcpp/rclcpp.hpp" -#include - -class CapabilitiesEventListener : public rclcpp::Node -{ -public: - using Event = capabilities2_msgs::msg::CapabilityEvent; - - CapabilitiesEventListener(const rclcpp::NodeOptions& options = rclcpp::NodeOptions()) : Node("capabilities2_events_listener", options) - { - // Create a subscription to the "topic" topic - RCLCPP_INFO(this->get_logger(), "Creating subscription to topic"); - - subscription_ = - this->create_subscription("/events", 10, std::bind(&CapabilitiesEventListener::topic_callback, this, std::placeholders::_1)); - } - -private: - /** - * @brief Constructs a message from the event and logs it - * - * @param msg - */ - void topic_callback(const Event& msg) const - { - std::string text; - - // Construct the text based on the event type and content - - // if the event contains and errornous element, we log it as an error - if (msg.type == Event::ERROR_ELEMENT) - { - text = "[" + msg.origin_node + "]" + "[" + msg.source.capability + "/" + std::to_string(msg.thread_id) + "] " + msg.content; - } - // If the event is at runner definition and logs on start event - else if (msg.type == Event::RUNNER_DEFINE and msg.event == Event::STARTED) - { - text = "[" + msg.origin_node + "]" + "[" + msg.source.capability + "] will trigger [" + msg.target.capability + "] on start"; - } - // If the event is at runner definition and logs on stop event - else if (msg.type == Event::RUNNER_DEFINE and msg.event == Event::STOPPED) - { - text = "[" + msg.origin_node + "]" + "[" + msg.source.capability + "] will trigger [" + msg.target.capability + "] on stop"; - } - // If the event is at runner definition and logs on failure event - else if (msg.type == Event::RUNNER_DEFINE and msg.event == Event::FAILED) - { - text = "[" + msg.origin_node + "]" + "[" + msg.source.capability + "] will trigger [" + msg.target.capability + "] on failure"; - } - // If the event is at runner definition and logs on success event - else if (msg.type == Event::RUNNER_DEFINE and msg.event == Event::SUCCEEDED) - { - text = "[" + msg.origin_node + "]" + "[" + msg.source.capability + "] will trigger [" + msg.target.capability + "] on success"; - } - // If the event is at runner execution and logs on start event from main thread - else if (msg.type == Event::RUNNER_EVENT and msg.event == Event::STARTED and msg.thread_id < 0) - { - text = "[" + msg.origin_node + "]" + "[" + msg.source.capability + "] triggering [" + msg.target.capability + "] on start"; - } - // If the event is at runner execution and logs on start event from worker thread - else if (msg.type == Event::RUNNER_EVENT and msg.event == Event::STARTED and msg.thread_id >= 0) - { - text = "[" + msg.origin_node + "]" + "[" + msg.source.capability + "/" + std::to_string(msg.thread_id) + "] triggering " + - msg.target.capability + "] on start"; - } - // If the event is at runner execution and logs on stop event from main thread - else if (msg.type == Event::RUNNER_EVENT and msg.event == Event::STOPPED and msg.thread_id < 0) - { - text = "[" + msg.origin_node + "]" + "[" + msg.source.capability + "] triggering [" + msg.target.capability + "] on stop"; - } - // If the event is at runner execution and logs on stop event from worker thread - else if (msg.type == Event::RUNNER_EVENT and msg.event == Event::STOPPED and msg.thread_id >= 0) - { - text = "[" + msg.origin_node + "]" + "[" + msg.source.capability + "/" + std::to_string(msg.thread_id) + "] triggering " + - msg.target.capability + "] on stop"; - } - // If the event is at runner execution and logs on failure event from main thread - else if (msg.type == Event::RUNNER_EVENT and msg.event == Event::FAILED and msg.thread_id < 0) - { - text = "[" + msg.origin_node + "]" + "[" + msg.source.capability + "] triggering [" + msg.target.capability + "] on failure"; - } - // If the event is at runner execution and logs on failure event from worker thread - else if (msg.type == Event::RUNNER_EVENT and msg.event == Event::FAILED and msg.thread_id >= 0) - { - text = "[" + msg.origin_node + "]" + "[" + msg.source.capability + "/" + std::to_string(msg.thread_id) + "] triggering " + - msg.target.capability + "] on failure"; - } - // If the event is at runner execution and logs on success event from main thread - else if (msg.type == Event::RUNNER_EVENT and msg.event == Event::SUCCEEDED and msg.thread_id < 0) - { - text = "[" + msg.origin_node + "]" + "[" + msg.source.capability + "] triggering [" + msg.target.capability + "] on success"; - } - // If the event is at runner execution and logs on success event from worker thread - else if (msg.type == Event::RUNNER_EVENT and msg.event == Event::SUCCEEDED and msg.thread_id >= 0) - { - text = "[" + msg.origin_node + "]" + "[" + msg.source.capability + "/" + std::to_string(msg.thread_id) + "] triggering " + - msg.target.capability + "] on success"; - } - // no capabilities means running on main node, thread id is -1 means on main thread - else if (msg.thread_id < 0 and msg.target.capability == "" and msg.source.capability == "") - { - text = "[" + msg.origin_node + "] " + msg.content; - } - // no capabilities means running on main node, thread id is >0 means on worker thread. - else if (msg.thread_id >= 0 and msg.target.capability == "" and msg.source.capability == "") - { - text = "[" + msg.origin_node + "]" + "[" + std::to_string(msg.thread_id) + "] " + msg.content; - } - // source capability is set, target capability is not set means msg is from within a capability, thread id is -1 means on main thread - else if (msg.thread_id < 0 and msg.target.capability == "" and msg.source.capability != "") - { - text = "[" + msg.origin_node + "]" + "[" + msg.source.capability + "] " + msg.content; - } - // source capability is set, target capability is not set means msg is from within a capability, thread id is >0 means on worker thread. - else if (msg.thread_id >= 0 and msg.target.capability == "" and msg.source.capability != "") - { - text = "[" + msg.origin_node + "]" + "[" + msg.source.capability + "/" + std::to_string(msg.thread_id) + "] " + msg.content; - } - - if (msg.type == Event::ERROR) - RCLCPP_ERROR(get_logger(), text.c_str()); - else if (msg.type == Event::ERROR_ELEMENT) - RCLCPP_ERROR(get_logger(), text.c_str()); - else if (msg.type == Event::DEBUG) - RCLCPP_DEBUG(get_logger(), text.c_str()); - else if (msg.type == Event::INFO) - RCLCPP_INFO(get_logger(), text.c_str()); - else if (msg.type == Event::RUNNER_DEFINE) - RCLCPP_INFO(get_logger(), text.c_str()); - else if (msg.type == Event::RUNNER_EVENT) - RCLCPP_INFO(get_logger(), text.c_str()); - else - RCLCPP_INFO(get_logger(), text.c_str()); - } - - rclcpp::Subscription::SharedPtr subscription_; -}; \ No newline at end of file diff --git a/capabilities2_events/include/capabilities2_events/event_types.hpp b/capabilities2_events/include/capabilities2_events/event_types.hpp deleted file mode 100644 index e0ddaf0..0000000 --- a/capabilities2_events/include/capabilities2_events/event_types.hpp +++ /dev/null @@ -1,44 +0,0 @@ -#pragma once - -namespace capabilities2 -{ -enum event_t -{ - IDLE, - STARTED, - STOPPED, - FAILED, - SUCCEEDED -}; - -/** - * @brief event definition - * - * Contains the informations about the event to be executed. It contains the interface, provider and parameters - */ -struct event -{ - std::string interface; - std::string provider; - std::string parameters; -}; - -/** - * @brief event options - * - * keeps track of events that are related to runner instances at various points of the - * plan - * @param on_started information about the capability to execute on start - * @param on_success information about the capability to execute on success - * @param on_failure information about the capability to execute on failure - * @param on_stopped information about the capability to execute on stop - */ -struct event_opts -{ - event on_started; - event on_success; - event on_failure; - event on_stopped; -}; - -} \ No newline at end of file diff --git a/capabilities2_events/launch/foxglove.launch.py b/capabilities2_events/launch/foxglove.launch.py deleted file mode 100644 index 80176f0..0000000 --- a/capabilities2_events/launch/foxglove.launch.py +++ /dev/null @@ -1,61 +0,0 @@ -from launch import LaunchDescription -from launch_ros.actions import Node -from launch.substitutions import LaunchConfiguration -from launch.actions import DeclareLaunchArgument - - -def generate_launch_description(): - # Declare launch arguments - launch_args = [ - DeclareLaunchArgument('port', default_value='8765'), - DeclareLaunchArgument('address', default_value='0.0.0.0'), - DeclareLaunchArgument('tls', default_value='false'), - DeclareLaunchArgument('certfile', default_value=''), - DeclareLaunchArgument('keyfile', default_value=''), - DeclareLaunchArgument('topic_whitelist', default_value="['/events']"), - # DeclareLaunchArgument('topic_whitelist', default_value="['.*']"), # Uncomment this line to whitelist all topics - DeclareLaunchArgument('param_whitelist', default_value="['.*']"), - DeclareLaunchArgument('service_whitelist', default_value="['.*']"), - DeclareLaunchArgument('client_topic_whitelist', default_value="['.*']"), - DeclareLaunchArgument('min_qos_depth', default_value='1'), - DeclareLaunchArgument('max_qos_depth', default_value='10'), - DeclareLaunchArgument('num_threads', default_value='0'), - DeclareLaunchArgument('send_buffer_limit', default_value='10000000'), - DeclareLaunchArgument('use_sim_time', default_value='false'), - DeclareLaunchArgument('capabilities', default_value="[clientPublish,parameters,parametersSubscribe,services,connectionGraph,assets]"), - DeclareLaunchArgument('include_hidden', default_value='false'), - DeclareLaunchArgument( - 'asset_uri_allowlist', - default_value=r"['^package://(?:[-\\w]+/)*[-\\w]+\\.(?:dae|fbx|glb|gltf|jpeg|jpg|mtl|obj|png|stl|tif|tiff|urdf|webp|xacro)$']" - ), - DeclareLaunchArgument('ignore_unresponsive_param_nodes', default_value='true'), - ] - - # Define the Node - foxglove_bridge_node = Node( - package='foxglove_bridge', - executable='foxglove_bridge', - parameters=[{ - 'port': LaunchConfiguration('port'), - 'address': LaunchConfiguration('address'), - 'tls': LaunchConfiguration('tls'), - 'certfile': LaunchConfiguration('certfile'), - 'keyfile': LaunchConfiguration('keyfile'), - 'topic_whitelist': LaunchConfiguration('topic_whitelist'), - 'param_whitelist': LaunchConfiguration('param_whitelist'), - 'service_whitelist': LaunchConfiguration('service_whitelist'), - 'client_topic_whitelist': LaunchConfiguration('client_topic_whitelist'), - 'min_qos_depth': LaunchConfiguration('min_qos_depth'), - 'max_qos_depth': LaunchConfiguration('max_qos_depth'), - 'num_threads': LaunchConfiguration('num_threads'), - 'send_buffer_limit': LaunchConfiguration('send_buffer_limit'), - 'use_sim_time': LaunchConfiguration('use_sim_time'), - 'capabilities': LaunchConfiguration('capabilities'), - 'include_hidden': LaunchConfiguration('include_hidden'), - 'asset_uri_allowlist': LaunchConfiguration('asset_uri_allowlist'), - 'ignore_unresponsive_param_nodes': LaunchConfiguration('ignore_unresponsive_param_nodes'), - }], - output='screen' - ) - - return LaunchDescription(launch_args + [foxglove_bridge_node]) \ No newline at end of file diff --git a/capabilities2_events/launch/listener.launch.py b/capabilities2_events/launch/listener.launch.py deleted file mode 100644 index 3f5951f..0000000 --- a/capabilities2_events/launch/listener.launch.py +++ /dev/null @@ -1,28 +0,0 @@ -''' -capabilities2_server launch file -''' - -import os -from launch import LaunchDescription -from launch_ros.actions import Node - - -def generate_launch_description(): - """Generate launch description for capabilities2 server - - Returns: - LaunchDescription: The launch description for capabilities2 events listener - """ - # create bridge composition - capabilities2 = Node( - package='capabilities2_events', - executable='capabilities2_events_node', - name='listener', - output='screen', - arguments=['--ros-args', '--log-level', 'info'] - ) - - # return - return LaunchDescription([ - capabilities2 - ]) diff --git a/capabilities2_events/package.xml b/capabilities2_events/package.xml deleted file mode 100644 index fb97ed2..0000000 --- a/capabilities2_events/package.xml +++ /dev/null @@ -1,28 +0,0 @@ - - - - capabilities2_events - 0.0.0 - TODO: Package description - - Kalana Ratnayake - Kalana Ratnayake - - Kalana Ratnayake - - TODO: License declaration - - ament_cmake - - ament_lint_auto - ament_lint_common - - rclcpp - capabilities2_msgs - rclcpp_components - foxglove_bridge - - - ament_cmake - - diff --git a/capabilities2_events/src/event_listener_node.cpp b/capabilities2_events/src/event_listener_node.cpp deleted file mode 100644 index 3eea375..0000000 --- a/capabilities2_events/src/event_listener_node.cpp +++ /dev/null @@ -1,17 +0,0 @@ -#include - -int main(int argc, char* argv[]) -{ - // Initialize the ROS 2 C++ client library - rclcpp::init(argc, argv); - - // Create a shared pointer to the CapabilitiesFabricClient - auto listener_node = std::make_shared(); - - // Spin the node to process callbacks - rclcpp::spin(listener_node); - - rclcpp::shutdown(); - - return 0; -} diff --git a/capabilities2_launch_proxy/capabilities2_launch_proxy/capabilities_launch_proxy.py b/capabilities2_launch_proxy/capabilities2_launch_proxy/capabilities_launch_proxy.py index 644bf8c..b1773c0 100755 --- a/capabilities2_launch_proxy/capabilities2_launch_proxy/capabilities_launch_proxy.py +++ b/capabilities2_launch_proxy/capabilities2_launch_proxy/capabilities_launch_proxy.py @@ -32,7 +32,6 @@ from launch.launch_description_sources import AnyLaunchDescriptionSource # from launch.some_entities_type import SomeEntitiesType from capabilities2_msgs.action import Launch -from capabilities2_msgs.msg import CapabilityEvent class CancelLaunchGoalEvent(Event): @@ -110,13 +109,6 @@ def __init__(self, node_name='capabilities_launch_proxy'): cancel_callback=self.cancel_cb ) - # cap event pub - self.event_pub = self.create_publisher( - CapabilityEvent, - '~/events', - 10 - ) - # create launch service self.launch_service = LaunchService() diff --git a/capabilities2_msgs/CMakeLists.txt b/capabilities2_msgs/CMakeLists.txt index 0fbf54e..529c0ce 100644 --- a/capabilities2_msgs/CMakeLists.txt +++ b/capabilities2_msgs/CMakeLists.txt @@ -17,7 +17,6 @@ find_package(std_msgs REQUIRED) set(msg_files "msg/Capability.msg" - "msg/CapabilityEvent.msg" "msg/CapabilitySpec.msg" "msg/Remapping.msg" "msg/RunningCapability.msg" diff --git a/capabilities2_msgs/action/Launch.action b/capabilities2_msgs/action/Launch.action index d4c26d4..c6e8c95 100644 --- a/capabilities2_msgs/action/Launch.action +++ b/capabilities2_msgs/action/Launch.action @@ -8,4 +8,3 @@ std_msgs/Header header --- # feedback std_msgs/Header header -capabilities2_msgs/CapabilityEvent event diff --git a/capabilities2_msgs/msg/CapabilityEvent.msg b/capabilities2_msgs/msg/CapabilityEvent.msg deleted file mode 100644 index 3c8d449..0000000 --- a/capabilities2_msgs/msg/CapabilityEvent.msg +++ /dev/null @@ -1,41 +0,0 @@ -std_msgs/Header header - -# Node name which published this event -string origin_node - -# Capability which this event pertains to -Capability source - -# Capability which this event targets at -Capability target - -# Thread id of the capability which this event pertains to -int8 thread_id - -# Events available -uint8 IDLE=0 -uint8 STARTED=1 #on_started -uint8 STOPPED=2 #on_stopped -uint8 FAILED=3 #on_failure -uint8 SUCCEEDED=4 #on_success -uint8 UNDEFINED=5 - -# Related event -uint8 event - -# Event types available -uint8 INFO=0 -uint8 DEBUG=1 -uint8 ERROR=2 -uint8 ERROR_ELEMENT=3 -uint8 RUNNER_DEFINE=4 -uint8 RUNNER_EVENT=5 - -# Related event type -uint8 type - -# status message -string content - -# PID of the related process -int32 pid diff --git a/capabilities2_runner/CMakeLists.txt b/capabilities2_runner/CMakeLists.txt index d866eee..74610f5 100644 --- a/capabilities2_runner/CMakeLists.txt +++ b/capabilities2_runner/CMakeLists.txt @@ -15,7 +15,8 @@ find_package(rclcpp REQUIRED) find_package(rclcpp_action REQUIRED) find_package(pluginlib REQUIRED) find_package(capabilities2_msgs REQUIRED) -find_package(capabilities2_events REQUIRED) +find_package(event_logger REQUIRED) +find_package(event_logger_msgs REQUIRED) find_package(tinyxml2_vendor REQUIRED) find_package(TinyXML2 REQUIRED) # provided by tinyxml2 upstream, or tinyxml2_vendor @@ -32,7 +33,8 @@ ament_target_dependencies(${PROJECT_NAME} rclcpp_action pluginlib capabilities2_msgs - capabilities2_events + event_logger + event_logger_msgs TinyXML2 ) diff --git a/capabilities2_runner/include/capabilities2_runner/runner_base.hpp b/capabilities2_runner/include/capabilities2_runner/runner_base.hpp index 2d30ee6..fd1076d 100644 --- a/capabilities2_runner/include/capabilities2_runner/runner_base.hpp +++ b/capabilities2_runner/include/capabilities2_runner/runner_base.hpp @@ -7,9 +7,9 @@ #include #include #include -#include -#include -#include +#include +#include +#include namespace capabilities2_runner { @@ -65,8 +65,8 @@ struct runner_opts class RunnerBase { public: - using Event = capabilities2_msgs::msg::CapabilityEvent; - using EventType = capabilities2::event_t; + using Event = event_logger_msgs::msg::Event; + using EventType = event_logger::event_t; RunnerBase() : run_config_() { @@ -141,7 +141,7 @@ class RunnerBase * * @return number of attached events */ - int attach_events(capabilities2::event_opts& event_option, + int attach_events(event_logger::event_opts& event_option, std::function triggerFunction) { info_("accepted event options with ID : " + std::to_string(insert_id)); @@ -453,10 +453,10 @@ class RunnerBase message.header.stamp = node_->now(); message.origin_node = "runners"; - message.source.capability = run_config_.interface; - message.source.provider = run_config_.provider; - message.target.capability = ""; - message.target.provider = ""; + message.source_capability = run_config_.interface; + message.source_provider = run_config_.provider; + message.target_capability = ""; + message.target_provider = ""; message.thread_id = thread_id; message.type = Event::INFO; message.content = text; @@ -472,10 +472,10 @@ class RunnerBase message.header.stamp = node_->now(); message.origin_node = "runners"; - message.source.capability = run_config_.interface; - message.source.provider = run_config_.provider; - message.target.capability = ""; - message.target.provider = ""; + message.source_capability = run_config_.interface; + message.source_provider = run_config_.provider; + message.target_capability = ""; + message.target_provider = ""; message.thread_id = thread_id; message.type = Event::ERROR; message.content = text; @@ -491,10 +491,10 @@ class RunnerBase message.header.stamp = node_->now(); message.origin_node = "runners"; - message.source.capability = run_config_.interface; - message.source.provider = run_config_.provider; - message.target.capability = ""; - message.target.provider = ""; + message.source_capability = run_config_.interface; + message.source_provider = run_config_.provider; + message.target_capability = ""; + message.target_provider = ""; message.thread_id = thread_id; message.type = Event::INFO; message.content = text + " : " + element; @@ -511,10 +511,10 @@ class RunnerBase message.header.stamp = node_->now(); message.origin_node = "runners"; - message.source.capability = run_config_.interface; - message.source.provider = run_config_.provider; - message.target.capability = target_capability; - message.target.provider = target_provider; + message.source_capability = run_config_.interface; + message.source_provider = run_config_.provider; + message.target_capability = target_capability; + message.target_provider = target_provider; message.thread_id = thread_id; message.type = Event::RUNNER_EVENT; message.pid = -1; @@ -557,7 +557,7 @@ class RunnerBase /** * @brief dictionary of events */ - std::map events; + std::map events; /** * @brief Last event tracker id to be inserted diff --git a/capabilities2_runner/package.xml b/capabilities2_runner/package.xml index b87f01e..248120c 100644 --- a/capabilities2_runner/package.xml +++ b/capabilities2_runner/package.xml @@ -21,7 +21,8 @@ pluginlib std_msgs capabilities2_msgs - capabilities2_events + event_logger + event_logger_msgs tinyxml2_vendor diff --git a/capabilities2_runner_audio/CMakeLists.txt b/capabilities2_runner_audio/CMakeLists.txt index 7f28773..14c53d0 100644 --- a/capabilities2_runner_audio/CMakeLists.txt +++ b/capabilities2_runner_audio/CMakeLists.txt @@ -18,7 +18,8 @@ find_package(pluginlib REQUIRED) find_package(hri_audio_msgs REQUIRED) find_package(capabilities2_msgs REQUIRED) find_package(capabilities2_runner REQUIRED) -find_package(capabilities2_events REQUIRED) +find_package(event_logger REQUIRED) +find_package(event_logger_msgs REQUIRED) find_package(tinyxml2_vendor REQUIRED) find_package(TinyXML2 REQUIRED) # provided by tinyxml2 upstream, or tinyxml2_vendor @@ -37,7 +38,8 @@ ament_target_dependencies(${PROJECT_NAME} hri_audio_msgs capabilities2_msgs capabilities2_runner - capabilities2_events + event_logger + event_logger_msgs TinyXML2 ) diff --git a/capabilities2_runner_audio/package.xml b/capabilities2_runner_audio/package.xml index b226bda..787289b 100644 --- a/capabilities2_runner_audio/package.xml +++ b/capabilities2_runner_audio/package.xml @@ -21,7 +21,8 @@ hri_audio_msgs capabilities2_msgs capabilities2_runner - capabilities2_events + event_logger + event_logger_msgs tinyxml2_vendor diff --git a/capabilities2_runner_bt/CMakeLists.txt b/capabilities2_runner_bt/CMakeLists.txt index 81c5fcc..1052e22 100644 --- a/capabilities2_runner_bt/CMakeLists.txt +++ b/capabilities2_runner_bt/CMakeLists.txt @@ -16,7 +16,8 @@ find_package(rclcpp_action REQUIRED) find_package(pluginlib REQUIRED) find_package(capabilities2_msgs REQUIRED) find_package(capabilities2_runner REQUIRED) -find_package(capabilities2_events REQUIRED) +find_package(event_logger REQUIRED) +find_package(event_logger_msgs REQUIRED) find_package(behaviortree_cpp REQUIRED) find_package(tinyxml2_vendor REQUIRED) find_package(TinyXML2 REQUIRED) # provided by tinyxml2 upstream, or tinyxml2_vendor @@ -35,7 +36,8 @@ ament_target_dependencies(${PROJECT_NAME} pluginlib capabilities2_msgs capabilities2_runner - capabilities2_events + event_logger + event_logger_msgs behaviortree_cpp TinyXML2 ) diff --git a/capabilities2_runner_bt/package.xml b/capabilities2_runner_bt/package.xml index 3dded60..d41e1c7 100644 --- a/capabilities2_runner_bt/package.xml +++ b/capabilities2_runner_bt/package.xml @@ -21,7 +21,8 @@ std_msgs capabilities2_msgs capabilities2_runner - capabilities2_events + event_logger + event_logger_msgs tinyxml2_vendor behaviortree_cpp diff --git a/capabilities2_runner_capabilities/CMakeLists.txt b/capabilities2_runner_capabilities/CMakeLists.txt index c90344e..57153c2 100644 --- a/capabilities2_runner_capabilities/CMakeLists.txt +++ b/capabilities2_runner_capabilities/CMakeLists.txt @@ -16,7 +16,8 @@ find_package(rclcpp_action REQUIRED) find_package(pluginlib REQUIRED) find_package(capabilities2_msgs REQUIRED) find_package(capabilities2_runner REQUIRED) -find_package(capabilities2_events REQUIRED) +find_package(event_logger REQUIRED) +find_package(event_logger_msgs REQUIRED) find_package(tinyxml2_vendor REQUIRED) find_package(TinyXML2 REQUIRED) # provided by tinyxml2 upstream, or tinyxml2_vendor @@ -34,7 +35,8 @@ ament_target_dependencies(${PROJECT_NAME} pluginlib capabilities2_msgs capabilities2_runner - capabilities2_events + event_logger + event_logger_msgs TinyXML2 ) diff --git a/capabilities2_runner_capabilities/package.xml b/capabilities2_runner_capabilities/package.xml index 8f1afe4..c97cfee 100644 --- a/capabilities2_runner_capabilities/package.xml +++ b/capabilities2_runner_capabilities/package.xml @@ -24,7 +24,8 @@ std_msgs capabilities2_msgs capabilities2_runner - capabilities2_events + event_logger + event_logger_msgs tinyxml2_vendor diff --git a/capabilities2_runner_fabric/CMakeLists.txt b/capabilities2_runner_fabric/CMakeLists.txt index f8caf64..15dc76e 100644 --- a/capabilities2_runner_fabric/CMakeLists.txt +++ b/capabilities2_runner_fabric/CMakeLists.txt @@ -17,7 +17,8 @@ find_package(pluginlib REQUIRED) find_package(fabric_msgs REQUIRED) find_package(capabilities2_msgs REQUIRED) find_package(capabilities2_runner REQUIRED) -find_package(capabilities2_events REQUIRED) +find_package(event_logger REQUIRED) +find_package(event_logger_msgs REQUIRED) find_package(tinyxml2_vendor REQUIRED) find_package(TinyXML2 REQUIRED) # provided by tinyxml2 upstream, or tinyxml2_vendor @@ -36,7 +37,8 @@ ament_target_dependencies(${PROJECT_NAME} fabric_msgs capabilities2_msgs capabilities2_runner - capabilities2_events + event_logger + event_logger_msgs TinyXML2 ) diff --git a/capabilities2_runner_fabric/package.xml b/capabilities2_runner_fabric/package.xml index c94f16b..d2b1197 100644 --- a/capabilities2_runner_fabric/package.xml +++ b/capabilities2_runner_fabric/package.xml @@ -24,7 +24,8 @@ std_msgs fabric_msgs capabilities2_runner - capabilities2_events + event_logger + event_logger_msgs tinyxml2_vendor diff --git a/capabilities2_runner_nav2/CMakeLists.txt b/capabilities2_runner_nav2/CMakeLists.txt index 29e59b1..61d91a3 100644 --- a/capabilities2_runner_nav2/CMakeLists.txt +++ b/capabilities2_runner_nav2/CMakeLists.txt @@ -20,7 +20,8 @@ find_package(tf2_ros REQUIRED) find_package(geometry_msgs REQUIRED) find_package(capabilities2_msgs REQUIRED) find_package(capabilities2_runner REQUIRED) -find_package(capabilities2_events REQUIRED) +find_package(event_logger REQUIRED) +find_package(event_logger_msgs REQUIRED) find_package(tinyxml2_vendor REQUIRED) find_package(TinyXML2 REQUIRED) # provided by tinyxml2 upstream, or tinyxml2_vendor @@ -42,7 +43,8 @@ ament_target_dependencies(${PROJECT_NAME} geometry_msgs capabilities2_msgs capabilities2_runner - capabilities2_events + event_logger + event_logger_msgs TinyXML2 ) diff --git a/capabilities2_runner_nav2/package.xml b/capabilities2_runner_nav2/package.xml index 110edc3..7222b62 100644 --- a/capabilities2_runner_nav2/package.xml +++ b/capabilities2_runner_nav2/package.xml @@ -21,7 +21,8 @@ geometry_msgs capabilities2_msgs capabilities2_runner - capabilities2_events + event_logger + event_logger_msgs tinyxml2_vendor ament_lint_auto diff --git a/capabilities2_runner_prompt/CMakeLists.txt b/capabilities2_runner_prompt/CMakeLists.txt index 6fb5ef2..b0a509c 100644 --- a/capabilities2_runner_prompt/CMakeLists.txt +++ b/capabilities2_runner_prompt/CMakeLists.txt @@ -17,7 +17,8 @@ find_package(pluginlib REQUIRED) find_package(prompt_msgs REQUIRED) find_package(capabilities2_msgs REQUIRED) find_package(capabilities2_runner REQUIRED) -find_package(capabilities2_events REQUIRED) +find_package(event_logger REQUIRED) +find_package(event_logger_msgs REQUIRED) find_package(tinyxml2_vendor REQUIRED) find_package(TinyXML2 REQUIRED) # provided by tinyxml2 upstream, or tinyxml2_vendor @@ -36,7 +37,8 @@ ament_target_dependencies(${PROJECT_NAME} prompt_msgs capabilities2_msgs capabilities2_runner - capabilities2_events + event_logger + event_logger_msgs TinyXML2 ) diff --git a/capabilities2_runner_prompt/package.xml b/capabilities2_runner_prompt/package.xml index 4e0301b..54160c3 100644 --- a/capabilities2_runner_prompt/package.xml +++ b/capabilities2_runner_prompt/package.xml @@ -20,7 +20,8 @@ prompt_msgs capabilities2_msgs capabilities2_runner - capabilities2_events + event_logger + event_logger_msgs tinyxml2_vendor ament_lint_auto diff --git a/capabilities2_server/CMakeLists.txt b/capabilities2_server/CMakeLists.txt index 384014b..0088818 100644 --- a/capabilities2_server/CMakeLists.txt +++ b/capabilities2_server/CMakeLists.txt @@ -19,7 +19,8 @@ find_package(bondcpp REQUIRED) find_package(pluginlib REQUIRED) find_package(capabilities2_msgs REQUIRED) find_package(capabilities2_runner REQUIRED) -find_package(capabilities2_events REQUIRED) +find_package(event_logger REQUIRED) +find_package(event_logger_msgs REQUIRED) find_package(tinyxml2_vendor REQUIRED) find_package(TinyXML2 REQUIRED) # provided by tinyxml2 upstream, or tinyxml2_vendor find_package(backward_ros REQUIRED) @@ -61,7 +62,8 @@ ament_target_dependencies(${PROJECT_NAME}_comp rclcpp_components capabilities2_msgs capabilities2_runner - capabilities2_events + event_logger + event_logger_msgs TinyXML2 SQLite3 yaml-cpp @@ -103,7 +105,8 @@ ament_target_dependencies(${PROJECT_NAME}_node pluginlib capabilities2_msgs capabilities2_runner - capabilities2_events + event_logger + event_logger_msgs TinyXML2 SQLite3 yaml-cpp diff --git a/capabilities2_server/include/capabilities2_server/capabilities_api.hpp b/capabilities2_server/include/capabilities2_server/capabilities_api.hpp index 7bab86a..6e9bf96 100644 --- a/capabilities2_server/include/capabilities2_server/capabilities_api.hpp +++ b/capabilities2_server/include/capabilities2_server/capabilities_api.hpp @@ -16,13 +16,14 @@ #include #include #include -#include -#include + +#include +#include +#include #include #include #include -#include #include #include @@ -239,7 +240,7 @@ class CapabilitiesAPI * @param capability capability from where the events originate * @param event_options event options for the capability */ - void set_triggers(const std::string& capability, capabilities2::event_opts& event_options) + void set_triggers(const std::string& capability, event_logger::event_opts& event_options) { try { diff --git a/capabilities2_server/include/capabilities2_server/capabilities_server.hpp b/capabilities2_server/include/capabilities2_server/capabilities_server.hpp index 0df4fb5..70f4a98 100644 --- a/capabilities2_server/include/capabilities2_server/capabilities_server.hpp +++ b/capabilities2_server/include/capabilities2_server/capabilities_server.hpp @@ -15,7 +15,6 @@ #include -#include #include #include @@ -34,8 +33,9 @@ #include #include -#include -#include +#include +#include +#include namespace capabilities2_server { @@ -318,35 +318,35 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI void configure_capability_cb(const std::shared_ptr req, std::shared_ptr res) { - capabilities2::event_opts event_options; + event_logger::event_opts event_options; event_options.on_started.interface = req->target_on_start.capability; event_options.on_started.provider = req->target_on_start.provider; event_options.on_started.parameters = req->target_on_start.parameters; event_->runner_define(req->source.capability, req->source.provider, req->target_on_start.capability, - req->target_on_start.provider, capabilities2_msgs::msg::CapabilityEvent::STARTED); + req->target_on_start.provider, event_logger_msgs::msg::Event::STARTED); event_options.on_failure.interface = req->target_on_failure.capability; event_options.on_failure.provider = req->target_on_failure.provider; event_options.on_failure.parameters = req->target_on_failure.parameters; event_->runner_define(req->source.capability, req->source.provider, req->target_on_failure.capability, - req->target_on_failure.provider, capabilities2_msgs::msg::CapabilityEvent::FAILED); + req->target_on_failure.provider, event_logger_msgs::msg::Event::FAILED); event_options.on_success.interface = req->target_on_success.capability; event_options.on_success.provider = req->target_on_success.provider; event_options.on_success.parameters = req->target_on_success.parameters; event_->runner_define(req->source.capability, req->source.provider, req->target_on_success.capability, - req->target_on_success.provider, capabilities2_msgs::msg::CapabilityEvent::SUCCEEDED); + req->target_on_success.provider, event_logger_msgs::msg::Event::SUCCEEDED); event_options.on_stopped.interface = req->target_on_stop.capability; event_options.on_stopped.provider = req->target_on_stop.provider; event_options.on_stopped.parameters = req->target_on_stop.parameters; event_->runner_define(req->source.capability, req->source.provider, req->target_on_stop.capability, - req->target_on_stop.provider, capabilities2_msgs::msg::CapabilityEvent::STOPPED); + req->target_on_stop.provider, event_logger_msgs::msg::Event::STOPPED); // setup triggers between parameters set_triggers(req->source.capability, event_options); diff --git a/capabilities2_server/include/capabilities2_server/runner_cache.hpp b/capabilities2_server/include/capabilities2_server/runner_cache.hpp index 94ac43e..1b20f79 100644 --- a/capabilities2_server/include/capabilities2_server/runner_cache.hpp +++ b/capabilities2_server/include/capabilities2_server/runner_cache.hpp @@ -9,7 +9,7 @@ #include #include #include -#include +#include namespace capabilities2_server { @@ -116,7 +116,7 @@ class RunnerCache * @param on_success on_success event with capability and parameters * @param on_stopped on_stop event with capability and parameters */ - void set_runner_triggers(const std::string& capability, capabilities2::event_opts& event_options) + void set_runner_triggers(const std::string& capability, event_logger::event_opts& event_options) { int event_count = runner_cache_[capability]->attach_events( event_options, std::bind(&capabilities2_server::RunnerCache::trigger_runner, this, std::placeholders::_1, diff --git a/capabilities2_server/package.xml b/capabilities2_server/package.xml index 226ae45..0a8ab8a 100644 --- a/capabilities2_server/package.xml +++ b/capabilities2_server/package.xml @@ -23,7 +23,8 @@ rclcpp_components capabilities2_msgs capabilities2_runner - capabilities2_events + event_logger + event_logger_msgs tinyxml2_vendor diff --git a/capabilities2_utils/include/capabilities2_utils/bond_client.hpp b/capabilities2_utils/include/capabilities2_utils/bond_client.hpp index ef7d675..3db48fd 100644 --- a/capabilities2_utils/include/capabilities2_utils/bond_client.hpp +++ b/capabilities2_utils/include/capabilities2_utils/bond_client.hpp @@ -2,7 +2,7 @@ #include #include #include -#include +#include class BondClient { From ff72cf99067ea66a9d6e6f1c17101dc901c6dde7 Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Mon, 7 Jul 2025 17:12:22 +1000 Subject: [PATCH 118/133] minor fix --- .../capabilities2_runner/runner_base.hpp | 33 ++++++++++--------- 1 file changed, 17 insertions(+), 16 deletions(-) diff --git a/capabilities2_runner/include/capabilities2_runner/runner_base.hpp b/capabilities2_runner/include/capabilities2_runner/runner_base.hpp index fd1076d..963d88a 100644 --- a/capabilities2_runner/include/capabilities2_runner/runner_base.hpp +++ b/capabilities2_runner/include/capabilities2_runner/runner_base.hpp @@ -8,6 +8,7 @@ #include #include #include +#include #include #include @@ -453,10 +454,10 @@ class RunnerBase message.header.stamp = node_->now(); message.origin_node = "runners"; - message.source_capability = run_config_.interface; - message.source_provider = run_config_.provider; - message.target_capability = ""; - message.target_provider = ""; + message.source.capability = run_config_.interface; + message.source.provider = run_config_.provider; + message.target.capability = ""; + message.target.provider = ""; message.thread_id = thread_id; message.type = Event::INFO; message.content = text; @@ -472,10 +473,10 @@ class RunnerBase message.header.stamp = node_->now(); message.origin_node = "runners"; - message.source_capability = run_config_.interface; - message.source_provider = run_config_.provider; - message.target_capability = ""; - message.target_provider = ""; + message.source.capability = run_config_.interface; + message.source.provider = run_config_.provider; + message.target.capability = ""; + message.target.provider = ""; message.thread_id = thread_id; message.type = Event::ERROR; message.content = text; @@ -491,10 +492,10 @@ class RunnerBase message.header.stamp = node_->now(); message.origin_node = "runners"; - message.source_capability = run_config_.interface; - message.source_provider = run_config_.provider; - message.target_capability = ""; - message.target_provider = ""; + message.source.capability = run_config_.interface; + message.source.provider = run_config_.provider; + message.target.capability = ""; + message.target.provider = ""; message.thread_id = thread_id; message.type = Event::INFO; message.content = text + " : " + element; @@ -511,10 +512,10 @@ class RunnerBase message.header.stamp = node_->now(); message.origin_node = "runners"; - message.source_capability = run_config_.interface; - message.source_provider = run_config_.provider; - message.target_capability = target_capability; - message.target_provider = target_provider; + message.source.capability = run_config_.interface; + message.source.provider = run_config_.provider; + message.target.capability = target_capability; + message.target.provider = target_provider; message.thread_id = thread_id; message.type = Event::RUNNER_EVENT; message.pid = -1; From fdd8cfaab17778fc08eb7f23d32a82bc07c36c6d Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Fri, 18 Jul 2025 17:23:45 +1000 Subject: [PATCH 119/133] minor update --- capabilities2_server/launch/server.launch.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/capabilities2_server/launch/server.launch.py b/capabilities2_server/launch/server.launch.py index 79dc98c..0ef7310 100644 --- a/capabilities2_server/launch/server.launch.py +++ b/capabilities2_server/launch/server.launch.py @@ -33,3 +33,5 @@ def generate_launch_description(): return LaunchDescription([ capabilities2 ]) + + From f7c804e74df4fb41e8094da4e6e18eb4d6fbaa08 Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Wed, 6 Aug 2025 18:58:17 +1000 Subject: [PATCH 120/133] updated syntax on fabric xml --- capabilities2_msgs/srv/ConfigureCapability.srv | 1 + .../include/capabilities2_server/capabilities_server.hpp | 8 ++++---- .../include/capabilities2_utils/connection.hpp | 1 + 3 files changed, 6 insertions(+), 4 deletions(-) diff --git a/capabilities2_msgs/srv/ConfigureCapability.srv b/capabilities2_msgs/srv/ConfigureCapability.srv index 66cf00c..726dab7 100644 --- a/capabilities2_msgs/srv/ConfigureCapability.srv +++ b/capabilities2_msgs/srv/ConfigureCapability.srv @@ -3,4 +3,5 @@ Capability target_on_start Capability target_on_stop Capability target_on_success Capability target_on_failure +string connection_description --- \ No newline at end of file diff --git a/capabilities2_server/include/capabilities2_server/capabilities_server.hpp b/capabilities2_server/include/capabilities2_server/capabilities_server.hpp index 70f4a98..32528b6 100644 --- a/capabilities2_server/include/capabilities2_server/capabilities_server.hpp +++ b/capabilities2_server/include/capabilities2_server/capabilities_server.hpp @@ -325,28 +325,28 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI event_options.on_started.parameters = req->target_on_start.parameters; event_->runner_define(req->source.capability, req->source.provider, req->target_on_start.capability, - req->target_on_start.provider, event_logger_msgs::msg::Event::STARTED); + req->target_on_start.provider, event_logger_msgs::msg::Event::STARTED, req->connection_description); event_options.on_failure.interface = req->target_on_failure.capability; event_options.on_failure.provider = req->target_on_failure.provider; event_options.on_failure.parameters = req->target_on_failure.parameters; event_->runner_define(req->source.capability, req->source.provider, req->target_on_failure.capability, - req->target_on_failure.provider, event_logger_msgs::msg::Event::FAILED); + req->target_on_failure.provider, event_logger_msgs::msg::Event::FAILED, req->connection_description); event_options.on_success.interface = req->target_on_success.capability; event_options.on_success.provider = req->target_on_success.provider; event_options.on_success.parameters = req->target_on_success.parameters; event_->runner_define(req->source.capability, req->source.provider, req->target_on_success.capability, - req->target_on_success.provider, event_logger_msgs::msg::Event::SUCCEEDED); + req->target_on_success.provider, event_logger_msgs::msg::Event::SUCCEEDED, req->connection_description); event_options.on_stopped.interface = req->target_on_stop.capability; event_options.on_stopped.provider = req->target_on_stop.provider; event_options.on_stopped.parameters = req->target_on_stop.parameters; event_->runner_define(req->source.capability, req->source.provider, req->target_on_stop.capability, - req->target_on_stop.provider, event_logger_msgs::msg::Event::STOPPED); + req->target_on_stop.provider, event_logger_msgs::msg::Event::STOPPED, req->connection_description); // setup triggers between parameters set_triggers(req->source.capability, event_options); diff --git a/capabilities2_utils/include/capabilities2_utils/connection.hpp b/capabilities2_utils/include/capabilities2_utils/connection.hpp index 8aa94f4..0d68d8e 100644 --- a/capabilities2_utils/include/capabilities2_utils/connection.hpp +++ b/capabilities2_utils/include/capabilities2_utils/connection.hpp @@ -25,6 +25,7 @@ namespace capabilities2 connection_t target_on_stop; connection_t target_on_success; connection_t target_on_failure; + std::string connection_description; }; } // namespace capabilities2 From ad29b276413e0d150fa1aad5373f489cf6ce5369 Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Tue, 12 Aug 2025 20:28:11 +1000 Subject: [PATCH 121/133] added new multiplexer runners --- .../capabilities2_runner/runner_base.hpp | 1 + .../input_multiplex_all_runner.hpp | 108 ++++++++++++++++++ .../input_multiplex_any_runner.hpp | 108 ++++++++++++++++++ capabilities2_runner_capabilities/plugins.xml | 12 ++ .../src/capabilities2_runner.cpp | 6 +- .../capabilities2_utils/connection.hpp | 5 +- 6 files changed, 237 insertions(+), 3 deletions(-) create mode 100644 capabilities2_runner_capabilities/include/capabilities2_runner_capabilities/input_multiplex_all_runner.hpp create mode 100644 capabilities2_runner_capabilities/include/capabilities2_runner_capabilities/input_multiplex_any_runner.hpp diff --git a/capabilities2_runner/include/capabilities2_runner/runner_base.hpp b/capabilities2_runner/include/capabilities2_runner/runner_base.hpp index 963d88a..3fe8e70 100644 --- a/capabilities2_runner/include/capabilities2_runner/runner_base.hpp +++ b/capabilities2_runner/include/capabilities2_runner/runner_base.hpp @@ -61,6 +61,7 @@ struct runner_opts std::string runner; std::string started_by; std::string pid; + int input_count; }; class RunnerBase diff --git a/capabilities2_runner_capabilities/include/capabilities2_runner_capabilities/input_multiplex_all_runner.hpp b/capabilities2_runner_capabilities/include/capabilities2_runner_capabilities/input_multiplex_all_runner.hpp new file mode 100644 index 0000000..322d8a3 --- /dev/null +++ b/capabilities2_runner_capabilities/include/capabilities2_runner_capabilities/input_multiplex_all_runner.hpp @@ -0,0 +1,108 @@ +#pragma once + +#include +#include + +namespace capabilities2_runner +{ +class InputMultiplexAllRunner : public RunnerBase +{ +public: + /** + * @brief Constructor which needs to be empty due to plugin semantics + */ + InputMultiplexAllRunner() : RunnerBase() + { + } + + /** + * @brief Starter function for starting the action runner + * + * @param node shared pointer to the capabilities node. Allows to use ros node related functionalities + * @param run_config runner configuration loaded from the yaml file + */ + virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config) override + { + init_base(node, run_config); + + info_("Starting InputMultiplexAllRunner with " + std::to_string(run_config.input_count) + " inputs."); + + expected_inputs_ = run_config.input_count; + + current_inputs_ = 0; + not_triggered_ = true; + } + + virtual void trigger(const std::string& parameters) override + { + current_inputs_ += 1; + + if (not_triggered_) + not_triggered_ = false; + + if (current_inputs_ == expected_inputs_) + { + info_("InputMultiplexAllRunner is has fullfilled the All condition with " + std::to_string(current_inputs_) + + " inputs."); + + executionThread = std::thread(&InputMultiplexAllRunner::execution, this, thread_id); + thread_id += 1; + } + else + { + info_("InputMultiplexAllRunner waiting. Only got " + std::to_string(current_inputs_ + 1) + "/" + + std::to_string(expected_inputs_) + " inputs."); + } + } + + virtual void execution(int id) + { + // trigger the events related to on_success state + if (events[execute_id].on_success.interface != "") + { + event_(EventType::SUCCEEDED, id, events[execute_id].on_success.interface, events[execute_id].on_success.provider); + triggerFunction_(events[execute_id].on_success.interface, + update_on_success(events[execute_id].on_success.parameters)); + } + // trigger the events related to on_failure state + else if (events[execute_id].on_failure.interface != "") + { + event_(EventType::FAILED, id, events[execute_id].on_failure.interface, events[execute_id].on_failure.provider); + triggerFunction_(events[execute_id].on_failure.interface, + update_on_failure(events[execute_id].on_failure.parameters)); + } + } + + /** + * @brief stop function to cease functionality and shutdown + * + */ + virtual void stop() override + { + // if the node pointer is empty then throw an error + // this means that the runner was not started and is being used out of order + + if (!node_) + throw runner_exception("cannot stop runner that was not started"); + + info_("stopping runner"); + } + + /** + * @brief Destructor + * + * Cleans up the thread if it is still running + */ + ~InputMultiplexAllRunner(); + +private: + bool not_triggered_; + + int expected_inputs_; + + int current_inputs_; + + std::thread executionThread; +}; + +} // namespace capabilities2_runner diff --git a/capabilities2_runner_capabilities/include/capabilities2_runner_capabilities/input_multiplex_any_runner.hpp b/capabilities2_runner_capabilities/include/capabilities2_runner_capabilities/input_multiplex_any_runner.hpp new file mode 100644 index 0000000..2adc201 --- /dev/null +++ b/capabilities2_runner_capabilities/include/capabilities2_runner_capabilities/input_multiplex_any_runner.hpp @@ -0,0 +1,108 @@ +#pragma once + +#include +#include + +namespace capabilities2_runner +{ +class InputMultiplexAnyRunner : public RunnerBase +{ +public: + /** + * @brief Constructor which needs to be empty due to plugin semantics + */ + InputMultiplexAnyRunner() : RunnerBase() + { + } + + /** + * @brief Starter function for starting the action runner + * + * @param node shared pointer to the capabilities node. Allows to use ros node related functionalities + * @param run_config runner configuration loaded from the yaml file + */ + virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config) override + { + init_base(node, run_config); + + info_("Starting InputMultiplexAnyRunner with " + std::to_string(run_config.input_count) + " inputs."); + + expected_inputs_ = run_config.input_count; + + current_inputs_ = 0; + not_triggered_ = true; + } + + virtual void trigger(const std::string& parameters) override + { + current_inputs_ += 1; + + if (not_triggered_) + not_triggered_ = false; + + if (current_inputs_ >= 0) + { + info_("InputMultiplexAnyRunner is has fullfilled the ANY condition with " + std::to_string(current_inputs_) + + " inputs."); + + executionThread = std::thread(&InputMultiplexAnyRunner::execution, this, thread_id); + thread_id += 1; + } + else + { + info_("InputMultiplexAnyRunner waiting. Only got " + std::to_string(current_inputs_ + 1) + "/" + + std::to_string(expected_inputs_) + " inputs."); + } + } + + virtual void execution(int id) + { + // trigger the events related to on_success state + if (events[execute_id].on_success.interface != "") + { + event_(EventType::SUCCEEDED, id, events[execute_id].on_success.interface, events[execute_id].on_success.provider); + triggerFunction_(events[execute_id].on_success.interface, + update_on_success(events[execute_id].on_success.parameters)); + } + // trigger the events related to on_failure state + else if (events[execute_id].on_failure.interface != "") + { + event_(EventType::FAILED, id, events[execute_id].on_failure.interface, events[execute_id].on_failure.provider); + triggerFunction_(events[execute_id].on_failure.interface, + update_on_failure(events[execute_id].on_failure.parameters)); + } + } + + /** + * @brief stop function to cease functionality and shutdown + * + */ + virtual void stop() override + { + // if the node pointer is empty then throw an error + // this means that the runner was not started and is being used out of order + + if (!node_) + throw runner_exception("cannot stop runner that was not started"); + + info_("stopping runner"); + } + + /** + * @brief Destructor + * + * Cleans up the thread if it is still running + */ + ~InputMultiplexAnyRunner(); + +private: + bool not_triggered_; + + int expected_inputs_; + + int current_inputs_; + + std::thread executionThread; +}; + +} // namespace capabilities2_runner diff --git a/capabilities2_runner_capabilities/plugins.xml b/capabilities2_runner_capabilities/plugins.xml index f4ff0f8..df235ea 100644 --- a/capabilities2_runner_capabilities/plugins.xml +++ b/capabilities2_runner_capabilities/plugins.xml @@ -4,4 +4,16 @@ A plugin that request capabilities from the capabilities server and transfers to an LLM + + + A plugin that Acts as a multiplexer for multiple input streams, allowing them to be processed in a single runner. + It can handle multiple input streams and route them to the appropriate output. + + + + + A plugin that Acts as a multiplexer for multiple input streams, allowing them to be processed in a single runner. + It can handle multiple input streams and route them to the appropriate output. + + diff --git a/capabilities2_runner_capabilities/src/capabilities2_runner.cpp b/capabilities2_runner_capabilities/src/capabilities2_runner.cpp index c6c35a5..a07e42f 100644 --- a/capabilities2_runner_capabilities/src/capabilities2_runner.cpp +++ b/capabilities2_runner_capabilities/src/capabilities2_runner.cpp @@ -1,6 +1,10 @@ #include #include #include +#include +#include // register runner plugins -PLUGINLIB_EXPORT_CLASS(capabilities2_runner::CapabilityGetRunner, capabilities2_runner::RunnerBase) +PLUGINLIB_EXPORT_CLASS(capabilities2_runner::CapabilityGetRunner, capabilities2_runner::RunnerBase); +PLUGINLIB_EXPORT_CLASS(capabilities2_runner::InputMultiplexAllRunner, capabilities2_runner::RunnerBase); +PLUGINLIB_EXPORT_CLASS(capabilities2_runner::InputMultiplexAnyRunner, capabilities2_runner::RunnerBase); diff --git a/capabilities2_utils/include/capabilities2_utils/connection.hpp b/capabilities2_utils/include/capabilities2_utils/connection.hpp index 0d68d8e..d6b68dc 100644 --- a/capabilities2_utils/include/capabilities2_utils/connection.hpp +++ b/capabilities2_utils/include/capabilities2_utils/connection.hpp @@ -14,8 +14,9 @@ namespace capabilities2 struct connection_t { - std::string runner; - std::string provider; + std::string runner = ""; + std::string provider = ""; + int input_count = 0; tinyxml2::XMLElement* parameters = nullptr; }; From 23cbb27dc3c99ee87e4548a5140d2a4ecde8fc20 Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Wed, 13 Aug 2025 17:35:10 +1000 Subject: [PATCH 122/133] updated capabilities2 server backend and refactored runners --- capabilities2_msgs/msg/Capability.msg | 2 + capabilities2_msgs/srv/UseCapability.srv | 1 + .../capabilities2_runner/runner_base.hpp | 21 ++++++ capabilities2_runner_capabilities/plugins.xml | 12 ---- .../src/capabilities2_runner.cpp | 4 -- capabilities2_runner_fabric/plugins.xml | 5 -- .../src/fabric_runners.cpp | 2 - capabilities2_runner_system/.clang-format | 64 +++++++++++++++++++ capabilities2_runner_system/CMakeLists.txt | 61 ++++++++++++++++++ .../completion_runner.hpp | 6 +- .../input_multiplex_all_runner.hpp | 37 +++++------ .../input_multiplex_any_runner.hpp | 38 ++++++----- capabilities2_runner_system/package.xml | 35 ++++++++++ capabilities2_runner_system/plugins.xml | 19 ++++++ .../src/capabilities2_runner.cpp | 10 +++ .../capabilities2_server/capabilities_api.hpp | 15 +++-- .../capabilities_server.hpp | 8 ++- .../capabilities2_server/runner_cache.hpp | 56 +++++++++++++--- system_capabilities/CMakeLists.txt | 16 +++++ system_capabilities/LICENSE | 17 +++++ .../interfaces/CompletionRunner.yaml | 13 ++++ .../interfaces/InputMultiplexAllRunner.yaml | 8 +++ .../interfaces/InputMultiplexAnyRunner.yaml | 8 +++ system_capabilities/package.xml | 48 ++++++++++++++ .../providers/CompletionRunner.yaml | 8 +++ .../providers/InputMultiplexAllRunner.yaml | 8 +++ .../providers/InputMultiplexAnyRunner.yaml | 8 +++ 27 files changed, 449 insertions(+), 81 deletions(-) create mode 100644 capabilities2_runner_system/.clang-format create mode 100644 capabilities2_runner_system/CMakeLists.txt rename {capabilities2_runner_fabric/include/capabilities2_runner_fabric => capabilities2_runner_system/include/capabilities2_runner_system}/completion_runner.hpp (89%) rename {capabilities2_runner_capabilities/include/capabilities2_runner_capabilities => capabilities2_runner_system/include/capabilities2_runner_system}/input_multiplex_all_runner.hpp (77%) rename {capabilities2_runner_capabilities/include/capabilities2_runner_capabilities => capabilities2_runner_system/include/capabilities2_runner_system}/input_multiplex_any_runner.hpp (77%) create mode 100644 capabilities2_runner_system/package.xml create mode 100644 capabilities2_runner_system/plugins.xml create mode 100644 capabilities2_runner_system/src/capabilities2_runner.cpp create mode 100644 system_capabilities/CMakeLists.txt create mode 100644 system_capabilities/LICENSE create mode 100644 system_capabilities/interfaces/CompletionRunner.yaml create mode 100644 system_capabilities/interfaces/InputMultiplexAllRunner.yaml create mode 100644 system_capabilities/interfaces/InputMultiplexAnyRunner.yaml create mode 100644 system_capabilities/package.xml create mode 100644 system_capabilities/providers/CompletionRunner.yaml create mode 100644 system_capabilities/providers/InputMultiplexAllRunner.yaml create mode 100644 system_capabilities/providers/InputMultiplexAnyRunner.yaml diff --git a/capabilities2_msgs/msg/Capability.msg b/capabilities2_msgs/msg/Capability.msg index 73384f0..5bf97a1 100644 --- a/capabilities2_msgs/msg/Capability.msg +++ b/capabilities2_msgs/msg/Capability.msg @@ -1,6 +1,8 @@ # Capability string capability + # Used provider string provider + # trigger parameters string parameters diff --git a/capabilities2_msgs/srv/UseCapability.srv b/capabilities2_msgs/srv/UseCapability.srv index fd6b98e..148ab0d 100644 --- a/capabilities2_msgs/srv/UseCapability.srv +++ b/capabilities2_msgs/srv/UseCapability.srv @@ -1,4 +1,5 @@ string capability string preferred_provider +int32 input_count string bond_id --- diff --git a/capabilities2_runner/include/capabilities2_runner/runner_base.hpp b/capabilities2_runner/include/capabilities2_runner/runner_base.hpp index 3fe8e70..6eb299c 100644 --- a/capabilities2_runner/include/capabilities2_runner/runner_base.hpp +++ b/capabilities2_runner/include/capabilities2_runner/runner_base.hpp @@ -132,6 +132,7 @@ class RunnerBase insert_id = 0; execute_id = -1; thread_id = 0; + current_inputs_ = 0; event_client_ = std::make_shared(node_, "runner", "/events"); } @@ -196,6 +197,16 @@ class RunnerBase return run_config_.pid; } + /** + * @brief Get the execution status of runner. + * + * @return `true` if execution is complete, `false` otherwise. + */ + const bool get_completion_status() const + { + return execution_complete_; + } + protected: /** * @brief Trigger process to be executed. @@ -576,6 +587,16 @@ class RunnerBase */ int thread_id; + /** + * @brief curent number of trigger signals received + */ + int current_inputs_; + + /** + * @brief system runner completion tracking + */ + bool execution_complete_; + /** * @brief pointer to XMLElement which contain parameters */ diff --git a/capabilities2_runner_capabilities/plugins.xml b/capabilities2_runner_capabilities/plugins.xml index df235ea..f4ff0f8 100644 --- a/capabilities2_runner_capabilities/plugins.xml +++ b/capabilities2_runner_capabilities/plugins.xml @@ -4,16 +4,4 @@ A plugin that request capabilities from the capabilities server and transfers to an LLM - - - A plugin that Acts as a multiplexer for multiple input streams, allowing them to be processed in a single runner. - It can handle multiple input streams and route them to the appropriate output. - - - - - A plugin that Acts as a multiplexer for multiple input streams, allowing them to be processed in a single runner. - It can handle multiple input streams and route them to the appropriate output. - - diff --git a/capabilities2_runner_capabilities/src/capabilities2_runner.cpp b/capabilities2_runner_capabilities/src/capabilities2_runner.cpp index a07e42f..bcdf964 100644 --- a/capabilities2_runner_capabilities/src/capabilities2_runner.cpp +++ b/capabilities2_runner_capabilities/src/capabilities2_runner.cpp @@ -1,10 +1,6 @@ #include #include #include -#include -#include // register runner plugins PLUGINLIB_EXPORT_CLASS(capabilities2_runner::CapabilityGetRunner, capabilities2_runner::RunnerBase); -PLUGINLIB_EXPORT_CLASS(capabilities2_runner::InputMultiplexAllRunner, capabilities2_runner::RunnerBase); -PLUGINLIB_EXPORT_CLASS(capabilities2_runner::InputMultiplexAnyRunner, capabilities2_runner::RunnerBase); diff --git a/capabilities2_runner_fabric/plugins.xml b/capabilities2_runner_fabric/plugins.xml index 5168f4b..07e8a06 100644 --- a/capabilities2_runner_fabric/plugins.xml +++ b/capabilities2_runner_fabric/plugins.xml @@ -1,9 +1,4 @@ - - - A plugin that notifies about the completion of the fabric to the action server - - A plugin that sets a new Fabric plan to the Fabric diff --git a/capabilities2_runner_fabric/src/fabric_runners.cpp b/capabilities2_runner_fabric/src/fabric_runners.cpp index 6167db2..703c1e1 100644 --- a/capabilities2_runner_fabric/src/fabric_runners.cpp +++ b/capabilities2_runner_fabric/src/fabric_runners.cpp @@ -1,8 +1,6 @@ #include #include -#include #include // register runner plugins -PLUGINLIB_EXPORT_CLASS(capabilities2_runner::FabricCompletionRunner, capabilities2_runner::RunnerBase) PLUGINLIB_EXPORT_CLASS(capabilities2_runner::FabricSetPlanRunner, capabilities2_runner::RunnerBase) diff --git a/capabilities2_runner_system/.clang-format b/capabilities2_runner_system/.clang-format new file mode 100644 index 0000000..d36804f --- /dev/null +++ b/capabilities2_runner_system/.clang-format @@ -0,0 +1,64 @@ + +BasedOnStyle: Google +AccessModifierOffset: -2 +ConstructorInitializerIndentWidth: 2 +AlignEscapedNewlinesLeft: false +AlignTrailingComments: true +AllowAllParametersOfDeclarationOnNextLine: false +AllowShortIfStatementsOnASingleLine: false +AllowShortLoopsOnASingleLine: false +AllowShortFunctionsOnASingleLine: None +AlwaysBreakTemplateDeclarations: true +AlwaysBreakBeforeMultilineStrings: false +BreakBeforeBinaryOperators: false +BreakBeforeTernaryOperators: false +BreakConstructorInitializersBeforeComma: true +BinPackParameters: true +ColumnLimit: 120 +ConstructorInitializerAllOnOneLineOrOnePerLine: true +DerivePointerBinding: false +PointerBindsToType: true +ExperimentalAutoDetectBinPacking: false +IndentCaseLabels: true +MaxEmptyLinesToKeep: 1 +NamespaceIndentation: None +ObjCSpaceBeforeProtocolList: true +PenaltyBreakBeforeFirstCallParameter: 19 +PenaltyBreakComment: 60 +PenaltyBreakString: 1 +PenaltyBreakFirstLessLess: 1000 +PenaltyExcessCharacter: 1000 +PenaltyReturnTypeOnItsOwnLine: 90 +SpacesBeforeTrailingComments: 2 +Cpp11BracedListStyle: false +Standard: Auto +IndentWidth: 2 +TabWidth: 2 +UseTab: Never +IndentFunctionDeclarationAfterType: false +SpacesInParentheses: false +SpacesInAngles: false +SpaceInEmptyParentheses: false +SpacesInCStyleCastParentheses: false +SpaceAfterControlStatementKeyword: true +SpaceBeforeAssignmentOperators: true +ContinuationIndentWidth: 4 +SortIncludes: false +SpaceAfterCStyleCast: false + +# Configure each individual brace in BraceWrapping +BreakBeforeBraces: Custom + +# Control of individual brace wrapping cases +BraceWrapping: { + AfterClass: 'true' + AfterControlStatement: 'true' + AfterEnum : 'true' + AfterFunction : 'true' + AfterNamespace : 'true' + AfterStruct : 'true' + AfterUnion : 'true' + BeforeCatch : 'true' + BeforeElse : 'true' + IndentBraces : 'false' +} diff --git a/capabilities2_runner_system/CMakeLists.txt b/capabilities2_runner_system/CMakeLists.txt new file mode 100644 index 0000000..6f4d9c3 --- /dev/null +++ b/capabilities2_runner_system/CMakeLists.txt @@ -0,0 +1,61 @@ +cmake_minimum_required(VERSION 3.8) +project(capabilities2_runner_system) + +# Default to C++17 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_action REQUIRED) +find_package(pluginlib REQUIRED) +find_package(capabilities2_msgs REQUIRED) +find_package(capabilities2_runner REQUIRED) +find_package(fabric_msgs REQUIRED) +find_package(event_logger REQUIRED) +find_package(event_logger_msgs REQUIRED) +find_package(tinyxml2_vendor REQUIRED) +find_package(TinyXML2 REQUIRED) # provided by tinyxml2 upstream, or tinyxml2_vendor + +include_directories( + include +) + +add_library(${PROJECT_NAME} SHARED + src/capabilities2_runner.cpp +) + +ament_target_dependencies(${PROJECT_NAME} + rclcpp + rclcpp_action + pluginlib + capabilities2_msgs + fabric_msgs + capabilities2_runner + event_logger + event_logger_msgs + TinyXML2 +) + +pluginlib_export_plugin_description_file(capabilities2_runner plugins.xml) + +install(TARGETS ${PROJECT_NAME} + EXPORT ${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +install(DIRECTORY include/ + DESTINATION include +) + +ament_export_include_directories(include) +ament_export_libraries(${PROJECT_NAME}) + +ament_package() diff --git a/capabilities2_runner_fabric/include/capabilities2_runner_fabric/completion_runner.hpp b/capabilities2_runner_system/include/capabilities2_runner_system/completion_runner.hpp similarity index 89% rename from capabilities2_runner_fabric/include/capabilities2_runner_fabric/completion_runner.hpp rename to capabilities2_runner_system/include/capabilities2_runner_system/completion_runner.hpp index 2475636..aeb2056 100644 --- a/capabilities2_runner_fabric/include/capabilities2_runner_fabric/completion_runner.hpp +++ b/capabilities2_runner_system/include/capabilities2_runner_system/completion_runner.hpp @@ -8,16 +8,16 @@ namespace capabilities2_runner { /** - * @brief fabric completion runner + * @brief completion runner * * This class is a wrapper around the capabilities2 service runner and is used to * call on the /capabilities_fabric/set_completion service, providing it as a * capability that notifys the completion of the fabric */ -class FabricCompletionRunner : public ServiceRunner +class CompletionRunner : public ServiceRunner { public: - FabricCompletionRunner() : ServiceRunner() + CompletionRunner() : ServiceRunner() { } diff --git a/capabilities2_runner_capabilities/include/capabilities2_runner_capabilities/input_multiplex_all_runner.hpp b/capabilities2_runner_system/include/capabilities2_runner_system/input_multiplex_all_runner.hpp similarity index 77% rename from capabilities2_runner_capabilities/include/capabilities2_runner_capabilities/input_multiplex_all_runner.hpp rename to capabilities2_runner_system/include/capabilities2_runner_system/input_multiplex_all_runner.hpp index 322d8a3..21e6298 100644 --- a/capabilities2_runner_capabilities/include/capabilities2_runner_capabilities/input_multiplex_all_runner.hpp +++ b/capabilities2_runner_system/include/capabilities2_runner_system/input_multiplex_all_runner.hpp @@ -25,36 +25,36 @@ class InputMultiplexAllRunner : public RunnerBase { init_base(node, run_config); - info_("Starting InputMultiplexAllRunner with " + std::to_string(run_config.input_count) + " inputs."); - - expected_inputs_ = run_config.input_count; - - current_inputs_ = 0; - not_triggered_ = true; + info_("started with " + std::to_string(run_config.input_count) + " inputs."); } + /** + * @brief trigger function to handle multiplexing of all inputs based on ALL condition + * + * @param parameters not used in this runner + */ virtual void trigger(const std::string& parameters) override { current_inputs_ += 1; - if (not_triggered_) - not_triggered_ = false; - - if (current_inputs_ == expected_inputs_) + if (current_inputs_ == run_config_.input_count) { - info_("InputMultiplexAllRunner is has fullfilled the All condition with " + std::to_string(current_inputs_) + - " inputs."); + info_("has fullfilled the All condition with " + std::to_string(current_inputs_) + " inputs."); executionThread = std::thread(&InputMultiplexAllRunner::execution, this, thread_id); thread_id += 1; } else { - info_("InputMultiplexAllRunner waiting. Only got " + std::to_string(current_inputs_ + 1) + "/" + - std::to_string(expected_inputs_) + " inputs."); + info_("only got " + std::to_string(current_inputs_) + "/" + std::to_string(run_config_.input_count) + " inputs."); } } + /** + * @brief Trigger process to be executed. + * + * @param id thread id + */ virtual void execution(int id) { // trigger the events related to on_success state @@ -96,12 +96,9 @@ class InputMultiplexAllRunner : public RunnerBase ~InputMultiplexAllRunner(); private: - bool not_triggered_; - - int expected_inputs_; - - int current_inputs_; - + /** + * @brief execution thread to handle the execution of the runner + */ std::thread executionThread; }; diff --git a/capabilities2_runner_capabilities/include/capabilities2_runner_capabilities/input_multiplex_any_runner.hpp b/capabilities2_runner_system/include/capabilities2_runner_system/input_multiplex_any_runner.hpp similarity index 77% rename from capabilities2_runner_capabilities/include/capabilities2_runner_capabilities/input_multiplex_any_runner.hpp rename to capabilities2_runner_system/include/capabilities2_runner_system/input_multiplex_any_runner.hpp index 2adc201..011cbf0 100644 --- a/capabilities2_runner_capabilities/include/capabilities2_runner_capabilities/input_multiplex_any_runner.hpp +++ b/capabilities2_runner_system/include/capabilities2_runner_system/input_multiplex_any_runner.hpp @@ -25,36 +25,36 @@ class InputMultiplexAnyRunner : public RunnerBase { init_base(node, run_config); - info_("Starting InputMultiplexAnyRunner with " + std::to_string(run_config.input_count) + " inputs."); - - expected_inputs_ = run_config.input_count; - - current_inputs_ = 0; - not_triggered_ = true; + info_("started with " + std::to_string(run_config.input_count) + " inputs."); } + /** + * @brief trigger function to handle multiplexing of all inputs based on ANY condition + * + * @param parameters not used in this runner + */ virtual void trigger(const std::string& parameters) override { current_inputs_ += 1; - if (not_triggered_) - not_triggered_ = false; - - if (current_inputs_ >= 0) + if (current_inputs_ > 0) { - info_("InputMultiplexAnyRunner is has fullfilled the ANY condition with " + std::to_string(current_inputs_) + - " inputs."); + info_("has fullfilled the ANY condition with " + std::to_string(current_inputs_) + " inputs."); executionThread = std::thread(&InputMultiplexAnyRunner::execution, this, thread_id); thread_id += 1; } else { - info_("InputMultiplexAnyRunner waiting. Only got " + std::to_string(current_inputs_ + 1) + "/" + - std::to_string(expected_inputs_) + " inputs."); + info_("only got " + std::to_string(current_inputs_) + "/" + std::to_string(run_config_.input_count) + " inputs."); } } + /** + * @brief Trigger process to be executed. + * + * @param id thread id + */ virtual void execution(int id) { // trigger the events related to on_success state @@ -64,6 +64,7 @@ class InputMultiplexAnyRunner : public RunnerBase triggerFunction_(events[execute_id].on_success.interface, update_on_success(events[execute_id].on_success.parameters)); } + // trigger the events related to on_failure state else if (events[execute_id].on_failure.interface != "") { @@ -96,12 +97,9 @@ class InputMultiplexAnyRunner : public RunnerBase ~InputMultiplexAnyRunner(); private: - bool not_triggered_; - - int expected_inputs_; - - int current_inputs_; - + /** + * @brief execution thread to handle the execution of the runner + */ std::thread executionThread; }; diff --git a/capabilities2_runner_system/package.xml b/capabilities2_runner_system/package.xml new file mode 100644 index 0000000..2f3f807 --- /dev/null +++ b/capabilities2_runner_system/package.xml @@ -0,0 +1,35 @@ + + + + capabilities2_runner_system + 0.0.0 + TODO: Package description + + + Kalana Ratnayake + Kalana Ratnayake + + Kalana Ratnayake + + TODO: License declaration + + ament_cmake + + ament_lint_auto + ament_lint_common + + rclcpp + rclcpp_action + pluginlib + std_msgs + capabilities2_msgs + capabilities2_runner + fabric_msgs + event_logger + event_logger_msgs + tinyxml2_vendor + + + ament_cmake + + diff --git a/capabilities2_runner_system/plugins.xml b/capabilities2_runner_system/plugins.xml new file mode 100644 index 0000000..41e8b96 --- /dev/null +++ b/capabilities2_runner_system/plugins.xml @@ -0,0 +1,19 @@ + + + + A plugin that Acts as a multiplexer for multiple input streams, allowing them to be processed in a single runner. + It can handle multiple input streams and route them to the appropriate output. + + + + + A plugin that Acts as a multiplexer for multiple input streams, allowing them to be processed in a single runner. + It can handle multiple input streams and route them to the appropriate output. + + + + + A plugin that notifies about the completion of the fabric to the action server + + + diff --git a/capabilities2_runner_system/src/capabilities2_runner.cpp b/capabilities2_runner_system/src/capabilities2_runner.cpp new file mode 100644 index 0000000..9d828da --- /dev/null +++ b/capabilities2_runner_system/src/capabilities2_runner.cpp @@ -0,0 +1,10 @@ +#include +#include +#include +#include +#include + +// register runner plugins +PLUGINLIB_EXPORT_CLASS(capabilities2_runner::InputMultiplexAllRunner, capabilities2_runner::RunnerBase); +PLUGINLIB_EXPORT_CLASS(capabilities2_runner::InputMultiplexAnyRunner, capabilities2_runner::RunnerBase); +PLUGINLIB_EXPORT_CLASS(capabilities2_runner::CompletionRunner, capabilities2_runner::RunnerBase) \ No newline at end of file diff --git a/capabilities2_server/include/capabilities2_server/capabilities_api.hpp b/capabilities2_server/include/capabilities2_server/capabilities_api.hpp index 6e9bf96..9901150 100644 --- a/capabilities2_server/include/capabilities2_server/capabilities_api.hpp +++ b/capabilities2_server/include/capabilities2_server/capabilities_api.hpp @@ -69,10 +69,12 @@ class CapabilitiesAPI * @param node ros node pointer of the ros server * @param capability capability name to be started * @param provider provider of the capability + * @param input_count number of inputs for the capability * * @return `true` if capability started successfully. else returns `false` */ - bool start_capability(rclcpp::Node::SharedPtr node, const std::string& capability, const std::string& provider) + bool start_capability(rclcpp::Node::SharedPtr node, const std::string& capability, const std::string& provider, + int input_count= 0) { // return value bool value = true; @@ -96,14 +98,14 @@ class CapabilitiesAPI // get the provider specification for the capability models::run_config_model_t run_config = cap_db_->get_run_config(provider); - // create a new runner - // this call implicitly starts the runner + // create a new runner, this call implicitly starts the runner // create a runner id which is the cap name to uniquely identify the runner // this means only one runner per capability name + // // TODO: consider the logic for multiple runners per capability try { - runner_cache_.add_runner(node, capability, run_config); + runner_cache_.add_runner(node, capability, run_config, input_count); event_->info("started capability: " + capability + " with provider: " + provider); @@ -220,18 +222,19 @@ class CapabilitiesAPI * @param node ros node pointer of the ros server * @param capability capability name to be started * @param provider provider of the capability + * @param input_count number of inputs for the capability * @param bond_id bond_id for the capability * * @return `true` if capability started successfully. else returns `false` */ bool use_capability(rclcpp::Node::SharedPtr node, const std::string& capability, const std::string& provider, - const std::string& bond_id) + int input_count, const std::string& bond_id) { // add bond to cache for capability bond_cache_.add_bond(capability, bond_id); // start the capability with the provider - return start_capability(node, capability, provider); + return start_capability(node, capability, provider, input_count); } /** diff --git a/capabilities2_server/include/capabilities2_server/capabilities_server.hpp b/capabilities2_server/include/capabilities2_server/capabilities_server.hpp index 32528b6..04d6397 100644 --- a/capabilities2_server/include/capabilities2_server/capabilities_server.hpp +++ b/capabilities2_server/include/capabilities2_server/capabilities_server.hpp @@ -308,8 +308,14 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI return; } + if (req->input_count > 0) + { + event_->error("use_capability: input count should be non zero"); + return; + } + // use capability with this bond - use_capability(shared_from_this(), req->capability, req->preferred_provider, req->bond_id); + use_capability(shared_from_this(), req->capability, req->preferred_provider, req->input_count, req->bond_id); // response is empty } diff --git a/capabilities2_server/include/capabilities2_server/runner_cache.hpp b/capabilities2_server/include/capabilities2_server/runner_cache.hpp index 1b20f79..4a21f21 100644 --- a/capabilities2_server/include/capabilities2_server/runner_cache.hpp +++ b/capabilities2_server/include/capabilities2_server/runner_cache.hpp @@ -50,10 +50,10 @@ class RunnerCache * @param run_config run_config of the runner to be loaded */ void add_runner(rclcpp::Node::SharedPtr node, const std::string& capability, - const models::run_config_model_t& run_config) + const models::run_config_model_t& run_config, int input_count = 0) { - // if the runner exists then throw an error - if (running(capability)) + // if the runner exists then throw an error preserving uniqueness + if (!is_system_capability(capability) && running(capability)) { // already running throw capabilities2_runner::runner_exception("capability is running already: " + capability); @@ -66,22 +66,28 @@ class RunnerCache throw capabilities2_runner::runner_exception("run config is not valid: " + YAML::Dump(run_config.to_yaml())); } - // create the runner - // add the runner to map - // if the spec runner contains a path to a launch file then use the launch file runner + // create the runner, add the runner to map, and if the spec runner contains a path to a launch file then use the + // launch file runner if (run_config.runner.find(".launch") != std::string::npos || run_config.runner.find("/") != std::string::npos || run_config.runner.find(".py") != std::string::npos) { runner_cache_[capability] = runner_loader_.createSharedInstance("capabilities2_runner::LaunchRunner"); } + else if (is_system_capability(capability)) + { + system_runner_cache_[capability].push_back(runner_loader_.createSharedInstance(run_config.runner)); + } else { - // use different runner types based on cap and provider specs runner_cache_[capability] = runner_loader_.createSharedInstance(run_config.runner); } + // add input count to the runner options + capabilities2_runner::runner_opts opts = run_config.to_runner_opts(); + opts.input_count = input_count; + // start the runner - runner_cache_[capability]->start(node, run_config.to_runner_opts()); + runner_cache_[capability]->start(node, opts); } /** @@ -130,6 +136,24 @@ class RunnerCache */ void remove_runner(const std::string& capability) { + if (is_system_capability(capability)) + { + // remove from system runner cache + auto it = system_runner_cache_.find(capability); + + // loop through runners and stop them + if (it != system_runner_cache_.end()) + { + for (auto& runner : it->second) + if (runner->get_completion_status()) + runner->stop(); + + // system_runner_cache_.erase(it); + } + } + + // ---- non-system: preserve your current unique semantics ---- + // find the runner in the cache if (!running(capability)) { @@ -225,11 +249,27 @@ class RunnerCache return runner_cache_.find(capability) != runner_cache_.end(); } + /** + * @brief Check if a capability is a system capability + * + * @param cap capability name + * @return true if it is a system capability + * @return false otherwise + */ + bool is_system_capability(const std::string& capability) + { + // (future-proof: new system runners still match) + return capability.rfind("system_capabilities/", 0) == 0; + } + private: // map capability to running model // capability / provider specs -> runner std::map> runner_cache_; + // system runner cache that allows duplicates + std::map>> system_runner_cache_; + // runner plugin loader pluginlib::ClassLoader runner_loader_; diff --git a/system_capabilities/CMakeLists.txt b/system_capabilities/CMakeLists.txt new file mode 100644 index 0000000..1d10d3b --- /dev/null +++ b/system_capabilities/CMakeLists.txt @@ -0,0 +1,16 @@ +cmake_minimum_required(VERSION 3.8) +project(system_capabilities) + +find_package(ament_cmake REQUIRED) + +# install interface files +install(DIRECTORY interfaces + DESTINATION share/${PROJECT_NAME} +) + +# install semantic interface files +install(DIRECTORY providers + DESTINATION share/${PROJECT_NAME} +) + +ament_package() diff --git a/system_capabilities/LICENSE b/system_capabilities/LICENSE new file mode 100644 index 0000000..30e8e2e --- /dev/null +++ b/system_capabilities/LICENSE @@ -0,0 +1,17 @@ +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE. diff --git a/system_capabilities/interfaces/CompletionRunner.yaml b/system_capabilities/interfaces/CompletionRunner.yaml new file mode 100644 index 0000000..903af66 --- /dev/null +++ b/system_capabilities/interfaces/CompletionRunner.yaml @@ -0,0 +1,13 @@ +%YAML 1.1 +--- +name: CompletionRunner +spec_version: 1.1 +spec_type: interface +description: "This capability notifies the completion of the capabilities fabric to the completion_server on the capabilities2 fabric + package. This is included on every fabric as the last capability to be triggered during connection idenification for the + fabric. A decision making authority such as an LLM does not need to include this in plans generated by it." +interface: + actions: + "/capabilities_fabric/set_completion": + type: "capabilities2_msgs::srv::CompleteFabric" + description: "Fabric completion notifier interface of the Capabilities fabric" \ No newline at end of file diff --git a/system_capabilities/interfaces/InputMultiplexAllRunner.yaml b/system_capabilities/interfaces/InputMultiplexAllRunner.yaml new file mode 100644 index 0000000..a587da6 --- /dev/null +++ b/system_capabilities/interfaces/InputMultiplexAllRunner.yaml @@ -0,0 +1,8 @@ +%YAML 1.1 +--- +name: InputMultiplexAllRunner +spec_version: 1.1 +spec_type: interface +description: "This capability combines the results of all input by multiplexing events into a single interface. It allows the robot to wait or + multiple parallel processes at once until completion. This is inserted by the system itself and a decision making authority such + as an LLM does not need to include this in plans generated by it." \ No newline at end of file diff --git a/system_capabilities/interfaces/InputMultiplexAnyRunner.yaml b/system_capabilities/interfaces/InputMultiplexAnyRunner.yaml new file mode 100644 index 0000000..3b06509 --- /dev/null +++ b/system_capabilities/interfaces/InputMultiplexAnyRunner.yaml @@ -0,0 +1,8 @@ +%YAML 1.1 +--- +name: InputMultiplexAnyRunner +spec_version: 1.1 +spec_type: interface +description: "This capability combines the results of any (at least one) input by multiplexing events into a single interface. It allows the + robot to wait or multiple parallel processes at once until completion. This is inserted by the system itself and a decision + making authority such as an LLM does not need to include this in plans generated by it." \ No newline at end of file diff --git a/system_capabilities/package.xml b/system_capabilities/package.xml new file mode 100644 index 0000000..6bbf60c --- /dev/null +++ b/system_capabilities/package.xml @@ -0,0 +1,48 @@ + + + + system_capabilities + 0.0.0 + TODO: Package description + kalana + MIT + + ament_cmake + + ament_lint_auto + ament_lint_common + + + ament_cmake + + + + interfaces/CompletionRunner.yaml + + + + providers/CompletionRunner.yaml + + + + + interfaces/InputMultiplexAllRunner.yaml + + + + providers/InputMultiplexAllRunner.yaml + + + + + + interfaces/InputMultiplexAnyRunner.yaml + + + + providers/InputMultiplexAnyRunner.yaml + + + + + diff --git a/system_capabilities/providers/CompletionRunner.yaml b/system_capabilities/providers/CompletionRunner.yaml new file mode 100644 index 0000000..ca84100 --- /dev/null +++ b/system_capabilities/providers/CompletionRunner.yaml @@ -0,0 +1,8 @@ +%YAML 1.1 +--- +name: CompletionRunner +spec_type: provider +spec_version: 1.1 +description: "The capability provider for the /capabilities_fabric/set_completion interface" +implements: system_capabilities/CompletionRunner +runner: capabilities2_runner::CompletionRunner diff --git a/system_capabilities/providers/InputMultiplexAllRunner.yaml b/system_capabilities/providers/InputMultiplexAllRunner.yaml new file mode 100644 index 0000000..b3edbfd --- /dev/null +++ b/system_capabilities/providers/InputMultiplexAllRunner.yaml @@ -0,0 +1,8 @@ +%YAML 1.1 +--- +name: InputMultiplexAllRunner +spec_type: provider +spec_version: 1.1 +description: "The capability provider for the system_capabilities/InputMultiplexAllRunner interface" +implements: system_capabilities/InputMultiplexAllRunner +runner: capabilities2_runner::InputMultiplexAllRunner \ No newline at end of file diff --git a/system_capabilities/providers/InputMultiplexAnyRunner.yaml b/system_capabilities/providers/InputMultiplexAnyRunner.yaml new file mode 100644 index 0000000..789ee28 --- /dev/null +++ b/system_capabilities/providers/InputMultiplexAnyRunner.yaml @@ -0,0 +1,8 @@ +%YAML 1.1 +--- +name: InputMultiplexAnyRunner +spec_type: provider +spec_version: 1.1 +description: "The capability provider for the system_capabilities/InputMultiplexAnyRunner interface" +implements: system_capabilities/InputMultiplexAnyRunner +runner: capabilities2_runner::InputMultiplexAnyRunner \ No newline at end of file From dd7e5d79205cdc14b04044044e4e6485fc8e6a4f Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Fri, 15 Aug 2025 17:04:59 +1000 Subject: [PATCH 123/133] minor modifiations after testing --- capabilities2_msgs/srv/UseCapability.srv | 1 - .../capabilities2_runner/runner_base.hpp | 3 +- .../completion_runner.hpp | 2 +- .../input_multiplex_all_runner.hpp | 92 ++++--------- .../input_multiplex_any_runner.hpp | 91 +++---------- .../multiplex_base_runner.hpp | 122 ++++++++++++++++++ .../capabilities2_server/capabilities_api.hpp | 12 +- .../capabilities_server.hpp | 8 +- .../capabilities2_server/runner_cache.hpp | 60 +-------- .../capabilities2_utils/connection.hpp | 1 - .../interfaces/CompletionRunner.yaml | 2 +- .../interfaces/InputMultiplexAllRunner.yaml | 7 +- .../interfaces/InputMultiplexAnyRunner.yaml | 7 +- .../providers/CompletionRunner.yaml | 2 +- 14 files changed, 195 insertions(+), 215 deletions(-) create mode 100644 capabilities2_runner_system/include/capabilities2_runner_system/multiplex_base_runner.hpp diff --git a/capabilities2_msgs/srv/UseCapability.srv b/capabilities2_msgs/srv/UseCapability.srv index 148ab0d..fd6b98e 100644 --- a/capabilities2_msgs/srv/UseCapability.srv +++ b/capabilities2_msgs/srv/UseCapability.srv @@ -1,5 +1,4 @@ string capability string preferred_provider -int32 input_count string bond_id --- diff --git a/capabilities2_runner/include/capabilities2_runner/runner_base.hpp b/capabilities2_runner/include/capabilities2_runner/runner_base.hpp index 6eb299c..a9115bf 100644 --- a/capabilities2_runner/include/capabilities2_runner/runner_base.hpp +++ b/capabilities2_runner/include/capabilities2_runner/runner_base.hpp @@ -141,10 +141,11 @@ class RunnerBase * @brief attach events to the runner * * @param event_option event_options related for the action + * @param triggerFunction external function that triggers capability runners * * @return number of attached events */ - int attach_events(event_logger::event_opts& event_option, + virtual int attach_events(event_logger::event_opts& event_option, std::function triggerFunction) { info_("accepted event options with ID : " + std::to_string(insert_id)); diff --git a/capabilities2_runner_system/include/capabilities2_runner_system/completion_runner.hpp b/capabilities2_runner_system/include/capabilities2_runner_system/completion_runner.hpp index aeb2056..8e5dcdc 100644 --- a/capabilities2_runner_system/include/capabilities2_runner_system/completion_runner.hpp +++ b/capabilities2_runner_system/include/capabilities2_runner_system/completion_runner.hpp @@ -11,7 +11,7 @@ namespace capabilities2_runner * @brief completion runner * * This class is a wrapper around the capabilities2 service runner and is used to - * call on the /capabilities_fabric/set_completion service, providing it as a + * call on the /fabric/set_completion service, providing it as a * capability that notifys the completion of the fabric */ class CompletionRunner : public ServiceRunner diff --git a/capabilities2_runner_system/include/capabilities2_runner_system/input_multiplex_all_runner.hpp b/capabilities2_runner_system/include/capabilities2_runner_system/input_multiplex_all_runner.hpp index 21e6298..6def71a 100644 --- a/capabilities2_runner_system/include/capabilities2_runner_system/input_multiplex_all_runner.hpp +++ b/capabilities2_runner_system/include/capabilities2_runner_system/input_multiplex_all_runner.hpp @@ -1,33 +1,19 @@ #pragma once -#include -#include +#include namespace capabilities2_runner { -class InputMultiplexAllRunner : public RunnerBase +class InputMultiplexAllRunner : public MultiplexBaseRunner { public: /** * @brief Constructor which needs to be empty due to plugin semantics */ - InputMultiplexAllRunner() : RunnerBase() + InputMultiplexAllRunner() : MultiplexBaseRunner() { } - /** - * @brief Starter function for starting the action runner - * - * @param node shared pointer to the capabilities node. Allows to use ros node related functionalities - * @param run_config runner configuration loaded from the yaml file - */ - virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config) override - { - init_base(node, run_config); - - info_("started with " + std::to_string(run_config.input_count) + " inputs."); - } - /** * @brief trigger function to handle multiplexing of all inputs based on ALL condition * @@ -35,71 +21,37 @@ class InputMultiplexAllRunner : public RunnerBase */ virtual void trigger(const std::string& parameters) override { - current_inputs_ += 1; + tinyxml2::XMLElement* parameters_ = convert_to_xml(parameters); + + int uid = 0; + int input_count = 0; - if (current_inputs_ == run_config_.input_count) + parameters_->QueryIntAttribute("input_count", &input_count); + parameters_->QueryIntAttribute("uid", &uid); + + if (input_count_tracker.find(uid) == input_count_tracker.end()) { - info_("has fullfilled the All condition with " + std::to_string(current_inputs_) + " inputs."); + input_count_tracker[uid] = 1; + expected_input_count[uid] = input_count; - executionThread = std::thread(&InputMultiplexAllRunner::execution, this, thread_id); - thread_id += 1; + info_("has started the All condition with " + std::to_string(input_count_tracker[uid]) + " inputs."); } else { - info_("only got " + std::to_string(current_inputs_) + "/" + std::to_string(run_config_.input_count) + " inputs."); - } - } + input_count_tracker[uid] += 1; - /** - * @brief Trigger process to be executed. - * - * @param id thread id - */ - virtual void execution(int id) - { - // trigger the events related to on_success state - if (events[execute_id].on_success.interface != "") - { - event_(EventType::SUCCEEDED, id, events[execute_id].on_success.interface, events[execute_id].on_success.provider); - triggerFunction_(events[execute_id].on_success.interface, - update_on_success(events[execute_id].on_success.parameters)); + info_("has received " + std::to_string(input_count_tracker[uid]) + "/" + + std::to_string(expected_input_count[uid]) + " inputs for ALL condition."); } - // trigger the events related to on_failure state - else if (events[execute_id].on_failure.interface != "") - { - event_(EventType::FAILED, id, events[execute_id].on_failure.interface, events[execute_id].on_failure.provider); - triggerFunction_(events[execute_id].on_failure.interface, - update_on_failure(events[execute_id].on_failure.parameters)); - } - } - - /** - * @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"); + if (input_count_tracker[uid] == expected_input_count[uid]) + { + info_("has fullfilled the All condition with " + std::to_string(input_count_tracker[uid]) + " inputs."); - info_("stopping runner"); + executionThreadPool[uid] = std::thread(&InputMultiplexAllRunner::execution, this, uid); + } } - /** - * @brief Destructor - * - * Cleans up the thread if it is still running - */ - ~InputMultiplexAllRunner(); - -private: - /** - * @brief execution thread to handle the execution of the runner - */ - std::thread executionThread; }; } // namespace capabilities2_runner diff --git a/capabilities2_runner_system/include/capabilities2_runner_system/input_multiplex_any_runner.hpp b/capabilities2_runner_system/include/capabilities2_runner_system/input_multiplex_any_runner.hpp index 011cbf0..5960a77 100644 --- a/capabilities2_runner_system/include/capabilities2_runner_system/input_multiplex_any_runner.hpp +++ b/capabilities2_runner_system/include/capabilities2_runner_system/input_multiplex_any_runner.hpp @@ -1,33 +1,19 @@ #pragma once -#include -#include +#include namespace capabilities2_runner { -class InputMultiplexAnyRunner : public RunnerBase +class InputMultiplexAnyRunner : public MultiplexBaseRunner { public: /** * @brief Constructor which needs to be empty due to plugin semantics */ - InputMultiplexAnyRunner() : RunnerBase() + InputMultiplexAnyRunner() : MultiplexBaseRunner() { } - /** - * @brief Starter function for starting the action runner - * - * @param node shared pointer to the capabilities node. Allows to use ros node related functionalities - * @param run_config runner configuration loaded from the yaml file - */ - virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config) override - { - init_base(node, run_config); - - info_("started with " + std::to_string(run_config.input_count) + " inputs."); - } - /** * @brief trigger function to handle multiplexing of all inputs based on ANY condition * @@ -35,72 +21,37 @@ class InputMultiplexAnyRunner : public RunnerBase */ virtual void trigger(const std::string& parameters) override { - current_inputs_ += 1; + tinyxml2::XMLElement* parameters_ = convert_to_xml(parameters); + + int uid = 0; + int input_count = 0; - if (current_inputs_ > 0) + parameters_->QueryIntAttribute("input_count", &input_count); + parameters_->QueryIntAttribute("uid", &uid); + + if (input_count_tracker.find(uid) == input_count_tracker.end()) { - info_("has fullfilled the ANY condition with " + std::to_string(current_inputs_) + " inputs."); + input_count_tracker[uid] = 1; + expected_input_count[uid] = input_count; - executionThread = std::thread(&InputMultiplexAnyRunner::execution, this, thread_id); - thread_id += 1; + info_("has started the ANY condition with " + std::to_string(input_count_tracker[uid]) + " inputs."); } else { - info_("only got " + std::to_string(current_inputs_) + "/" + std::to_string(run_config_.input_count) + " inputs."); - } - } + input_count_tracker[uid] += 1; - /** - * @brief Trigger process to be executed. - * - * @param id thread id - */ - virtual void execution(int id) - { - // trigger the events related to on_success state - if (events[execute_id].on_success.interface != "") - { - event_(EventType::SUCCEEDED, id, events[execute_id].on_success.interface, events[execute_id].on_success.provider); - triggerFunction_(events[execute_id].on_success.interface, - update_on_success(events[execute_id].on_success.parameters)); + info_("has received " + std::to_string(input_count_tracker[uid]) + "/" + + std::to_string(expected_input_count[uid]) + " inputs for ANY condition."); } - // trigger the events related to on_failure state - else if (events[execute_id].on_failure.interface != "") + if (input_count_tracker[uid] > 0) { - event_(EventType::FAILED, id, events[execute_id].on_failure.interface, events[execute_id].on_failure.provider); - triggerFunction_(events[execute_id].on_failure.interface, - update_on_failure(events[execute_id].on_failure.parameters)); - } - } - - /** - * @brief stop function to cease functionality and shutdown - * - */ - virtual void stop() override - { - // if the node pointer is empty then throw an error - // this means that the runner was not started and is being used out of order - - if (!node_) - throw runner_exception("cannot stop runner that was not started"); + info_("has fullfilled the ANY condition with " + std::to_string(input_count_tracker[uid]) + " inputs."); - info_("stopping runner"); + executionThreadPool[uid] = std::thread(&InputMultiplexAnyRunner::execution, this, uid); + } } - /** - * @brief Destructor - * - * Cleans up the thread if it is still running - */ - ~InputMultiplexAnyRunner(); - -private: - /** - * @brief execution thread to handle the execution of the runner - */ - std::thread executionThread; }; } // namespace capabilities2_runner diff --git a/capabilities2_runner_system/include/capabilities2_runner_system/multiplex_base_runner.hpp b/capabilities2_runner_system/include/capabilities2_runner_system/multiplex_base_runner.hpp new file mode 100644 index 0000000..c02739d --- /dev/null +++ b/capabilities2_runner_system/include/capabilities2_runner_system/multiplex_base_runner.hpp @@ -0,0 +1,122 @@ +#pragma once + +#include +#include +#include +#include + +namespace capabilities2_runner +{ +class MultiplexBaseRunner : public RunnerBase +{ +public: + /** + * @brief Constructor which needs to be empty due to plugin semantics + */ + MultiplexBaseRunner() : RunnerBase() + { + } + + /** + * @brief Starter function for starting the action runner + * + * @param node shared pointer to the capabilities node. Allows to use ros node related functionalities + * @param run_config runner configuration loaded from the yaml file + */ + virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config) override + { + init_base(node, run_config); + } + + /** + * @brief Trigger process to be executed. + * + * @param uid unique identifier for the execution + */ + virtual void execution(int uid) + { + info_("execution started for uid: " + std::to_string(uid)); + + // trigger the events related to on_success state + if (events[uid].on_success.interface != "") + { + event_(EventType::SUCCEEDED, uid, events[uid].on_success.interface, events[uid].on_success.provider); + triggerFunction_(events[uid].on_success.interface, update_on_success(events[uid].on_success.parameters)); + } + // trigger the events related to on_failure state + else if (events[uid].on_failure.interface != "") + { + event_(EventType::FAILED, uid, events[uid].on_failure.interface, events[uid].on_failure.provider); + triggerFunction_(events[uid].on_failure.interface, update_on_failure(events[uid].on_failure.parameters)); + } + } + + /** + * @brief attach events to the runner + * + * @param event_option event_options related for the action + * @param triggerFunction external function that triggers capability runners + * + * @return number of attached events + */ + virtual int attach_events(event_logger::event_opts& event_option, + std::function triggerFunction) override + { + info_("accepted event options with ID : " + std::to_string(insert_id)); + + triggerFunction_ = triggerFunction; + + tinyxml2::XMLElement* on_success_params = convert_to_xml(event_option.on_success.parameters); + + int uid = NULL; + + // extract the uid from the event options from whatever runner is present by looping + if (event_option.on_success.interface != "") + { + tinyxml2::XMLElement* on_success_params = convert_to_xml(event_option.on_success.parameters); + on_success_params->QueryIntAttribute("uid", &uid); + } + else if (event_option.on_failure.interface != "") + { + tinyxml2::XMLElement* on_failure_params = convert_to_xml(event_option.on_failure.parameters); + on_failure_params->QueryIntAttribute("uid", &uid); + } + else if (event_option.on_started.interface != "") + { + tinyxml2::XMLElement* on_started_params = convert_to_xml(event_option.on_started.parameters); + on_started_params->QueryIntAttribute("uid", &uid); + } + else if (event_option.on_stopped.interface != "") + { + tinyxml2::XMLElement* on_stopped_params = convert_to_xml(event_option.on_stopped.parameters); + on_stopped_params->QueryIntAttribute("uid", &uid); + } + + events[uid] = event_option; + + return uid; + } + + /** + * @brief stop function to cease functionality and shutdown + * + */ + virtual void stop() override + { + // if the node pointer is empty then throw an error + // this means that the runner was not started and is being used out of order + + if (!node_) + throw runner_exception("cannot stop runner that was not started"); + + info_("stopping runner"); + } + +protected: + // input count tracker + std::map input_count_tracker; + + // expected input count + std::map expected_input_count; +}; +} // namespace capabilities2_runner \ No newline at end of file diff --git a/capabilities2_server/include/capabilities2_server/capabilities_api.hpp b/capabilities2_server/include/capabilities2_server/capabilities_api.hpp index 9901150..97b8b24 100644 --- a/capabilities2_server/include/capabilities2_server/capabilities_api.hpp +++ b/capabilities2_server/include/capabilities2_server/capabilities_api.hpp @@ -69,12 +69,10 @@ class CapabilitiesAPI * @param node ros node pointer of the ros server * @param capability capability name to be started * @param provider provider of the capability - * @param input_count number of inputs for the capability * * @return `true` if capability started successfully. else returns `false` */ - bool start_capability(rclcpp::Node::SharedPtr node, const std::string& capability, const std::string& provider, - int input_count= 0) + bool start_capability(rclcpp::Node::SharedPtr node, const std::string& capability, const std::string& provider) { // return value bool value = true; @@ -105,7 +103,7 @@ class CapabilitiesAPI // TODO: consider the logic for multiple runners per capability try { - runner_cache_.add_runner(node, capability, run_config, input_count); + runner_cache_.add_runner(node, capability, run_config); event_->info("started capability: " + capability + " with provider: " + provider); @@ -114,6 +112,7 @@ class CapabilitiesAPI catch (const capabilities2_runner::runner_exception& e) { event_->error("could not start runner: " + std::string(e.what())); + return false; } } @@ -222,19 +221,18 @@ class CapabilitiesAPI * @param node ros node pointer of the ros server * @param capability capability name to be started * @param provider provider of the capability - * @param input_count number of inputs for the capability * @param bond_id bond_id for the capability * * @return `true` if capability started successfully. else returns `false` */ bool use_capability(rclcpp::Node::SharedPtr node, const std::string& capability, const std::string& provider, - int input_count, const std::string& bond_id) + const std::string& bond_id) { // add bond to cache for capability bond_cache_.add_bond(capability, bond_id); // start the capability with the provider - return start_capability(node, capability, provider, input_count); + return start_capability(node, capability, provider); } /** diff --git a/capabilities2_server/include/capabilities2_server/capabilities_server.hpp b/capabilities2_server/include/capabilities2_server/capabilities_server.hpp index 04d6397..32528b6 100644 --- a/capabilities2_server/include/capabilities2_server/capabilities_server.hpp +++ b/capabilities2_server/include/capabilities2_server/capabilities_server.hpp @@ -308,14 +308,8 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI return; } - if (req->input_count > 0) - { - event_->error("use_capability: input count should be non zero"); - return; - } - // use capability with this bond - use_capability(shared_from_this(), req->capability, req->preferred_provider, req->input_count, req->bond_id); + use_capability(shared_from_this(), req->capability, req->preferred_provider, req->bond_id); // response is empty } diff --git a/capabilities2_server/include/capabilities2_server/runner_cache.hpp b/capabilities2_server/include/capabilities2_server/runner_cache.hpp index 4a21f21..4c1aac1 100644 --- a/capabilities2_server/include/capabilities2_server/runner_cache.hpp +++ b/capabilities2_server/include/capabilities2_server/runner_cache.hpp @@ -50,14 +50,12 @@ class RunnerCache * @param run_config run_config of the runner to be loaded */ void add_runner(rclcpp::Node::SharedPtr node, const std::string& capability, - const models::run_config_model_t& run_config, int input_count = 0) + const models::run_config_model_t& run_config) { // if the runner exists then throw an error preserving uniqueness - if (!is_system_capability(capability) && running(capability)) + if (running(capability)) { - // already running throw capabilities2_runner::runner_exception("capability is running already: " + capability); - // return; } // check if run config is valid @@ -73,21 +71,13 @@ class RunnerCache { runner_cache_[capability] = runner_loader_.createSharedInstance("capabilities2_runner::LaunchRunner"); } - else if (is_system_capability(capability)) - { - system_runner_cache_[capability].push_back(runner_loader_.createSharedInstance(run_config.runner)); - } else { runner_cache_[capability] = runner_loader_.createSharedInstance(run_config.runner); } - // add input count to the runner options - capabilities2_runner::runner_opts opts = run_config.to_runner_opts(); - opts.input_count = input_count; - // start the runner - runner_cache_[capability]->start(node, opts); + runner_cache_[capability]->start(node, run_config.to_runner_opts()); } /** @@ -124,9 +114,9 @@ class RunnerCache */ void set_runner_triggers(const std::string& capability, event_logger::event_opts& event_options) { - int event_count = runner_cache_[capability]->attach_events( - event_options, std::bind(&capabilities2_server::RunnerCache::trigger_runner, this, std::placeholders::_1, - std::placeholders::_2)); + runner_cache_[capability]->attach_events(event_options, + std::bind(&capabilities2_server::RunnerCache::trigger_runner, this, + std::placeholders::_1, std::placeholders::_2)); } /** @@ -136,30 +126,10 @@ class RunnerCache */ void remove_runner(const std::string& capability) { - if (is_system_capability(capability)) - { - // remove from system runner cache - auto it = system_runner_cache_.find(capability); - - // loop through runners and stop them - if (it != system_runner_cache_.end()) - { - for (auto& runner : it->second) - if (runner->get_completion_status()) - runner->stop(); - - // system_runner_cache_.erase(it); - } - } - - // ---- non-system: preserve your current unique semantics ---- - - // find the runner in the cache + // find the runner in the cache and if not found then throw an error if (!running(capability)) { - // not found so nothing to do throw capabilities2_runner::runner_exception("capability runner not found: " + capability); - // return; } // safely stop the runner @@ -249,27 +219,11 @@ class RunnerCache return runner_cache_.find(capability) != runner_cache_.end(); } - /** - * @brief Check if a capability is a system capability - * - * @param cap capability name - * @return true if it is a system capability - * @return false otherwise - */ - bool is_system_capability(const std::string& capability) - { - // (future-proof: new system runners still match) - return capability.rfind("system_capabilities/", 0) == 0; - } - private: // map capability to running model // capability / provider specs -> runner std::map> runner_cache_; - // system runner cache that allows duplicates - std::map>> system_runner_cache_; - // runner plugin loader pluginlib::ClassLoader runner_loader_; diff --git a/capabilities2_utils/include/capabilities2_utils/connection.hpp b/capabilities2_utils/include/capabilities2_utils/connection.hpp index d6b68dc..c88ede3 100644 --- a/capabilities2_utils/include/capabilities2_utils/connection.hpp +++ b/capabilities2_utils/include/capabilities2_utils/connection.hpp @@ -16,7 +16,6 @@ namespace capabilities2 { std::string runner = ""; std::string provider = ""; - int input_count = 0; tinyxml2::XMLElement* parameters = nullptr; }; diff --git a/system_capabilities/interfaces/CompletionRunner.yaml b/system_capabilities/interfaces/CompletionRunner.yaml index 903af66..d34d09f 100644 --- a/system_capabilities/interfaces/CompletionRunner.yaml +++ b/system_capabilities/interfaces/CompletionRunner.yaml @@ -8,6 +8,6 @@ description: "This capability notifies the completion of the capabilities fabric fabric. A decision making authority such as an LLM does not need to include this in plans generated by it." interface: actions: - "/capabilities_fabric/set_completion": + "/fabric/set_completion": type: "capabilities2_msgs::srv::CompleteFabric" description: "Fabric completion notifier interface of the Capabilities fabric" \ No newline at end of file diff --git a/system_capabilities/interfaces/InputMultiplexAllRunner.yaml b/system_capabilities/interfaces/InputMultiplexAllRunner.yaml index a587da6..0ab62cc 100644 --- a/system_capabilities/interfaces/InputMultiplexAllRunner.yaml +++ b/system_capabilities/interfaces/InputMultiplexAllRunner.yaml @@ -5,4 +5,9 @@ spec_version: 1.1 spec_type: interface description: "This capability combines the results of all input by multiplexing events into a single interface. It allows the robot to wait or multiple parallel processes at once until completion. This is inserted by the system itself and a decision making authority such - as an LLM does not need to include this in plans generated by it." \ No newline at end of file + as an LLM does not need to include this in plans generated by it." +interface: + actions: + "empty": + type: std_srvs/action/Empty + description: empty. not used \ No newline at end of file diff --git a/system_capabilities/interfaces/InputMultiplexAnyRunner.yaml b/system_capabilities/interfaces/InputMultiplexAnyRunner.yaml index 3b06509..ff4313b 100644 --- a/system_capabilities/interfaces/InputMultiplexAnyRunner.yaml +++ b/system_capabilities/interfaces/InputMultiplexAnyRunner.yaml @@ -5,4 +5,9 @@ spec_version: 1.1 spec_type: interface description: "This capability combines the results of any (at least one) input by multiplexing events into a single interface. It allows the robot to wait or multiple parallel processes at once until completion. This is inserted by the system itself and a decision - making authority such as an LLM does not need to include this in plans generated by it." \ No newline at end of file + making authority such as an LLM does not need to include this in plans generated by it." +interface: + actions: + "empty": + type: std_srvs/action/Empty + description: empty. not used \ No newline at end of file diff --git a/system_capabilities/providers/CompletionRunner.yaml b/system_capabilities/providers/CompletionRunner.yaml index ca84100..3fb38e6 100644 --- a/system_capabilities/providers/CompletionRunner.yaml +++ b/system_capabilities/providers/CompletionRunner.yaml @@ -3,6 +3,6 @@ name: CompletionRunner spec_type: provider spec_version: 1.1 -description: "The capability provider for the /capabilities_fabric/set_completion interface" +description: "The capability provider for the /fabric/set_completion interface" implements: system_capabilities/CompletionRunner runner: capabilities2_runner::CompletionRunner From 4657c750ee412153849e6793ecb5a80da90b0687 Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Sun, 17 Aug 2025 20:32:43 +1000 Subject: [PATCH 124/133] updated event system to be compatible with multiplexing runners used in recovery and parallel routiens --- .../srv/ConfigureCapability.srv | 1 + capabilities2_runner/CMakeLists.txt | 2 + .../capabilities2_runner/action_runner.hpp | 37 ++++----- .../capabilities2_runner/runner_base.hpp | 53 +++++------- .../capabilities2_runner/service_runner.hpp | 35 ++++---- .../capabilities2_runner/topic_runner.hpp | 32 +++----- capabilities2_runner/package.xml | 1 + capabilities2_runner_audio/CMakeLists.txt | 2 + capabilities2_runner_audio/package.xml | 1 + capabilities2_runner_bt/CMakeLists.txt | 2 + capabilities2_runner_bt/package.xml | 1 + .../CMakeLists.txt | 2 + capabilities2_runner_fabric/CMakeLists.txt | 2 + capabilities2_runner_nav2/CMakeLists.txt | 2 + capabilities2_runner_prompt/CMakeLists.txt | 2 + capabilities2_runner_system/CMakeLists.txt | 2 + .../input_multiplex_all_runner.hpp | 24 +++--- .../input_multiplex_any_runner.hpp | 37 +++++---- .../multiplex_base_runner.hpp | 81 ++++++------------- capabilities2_runner_system/package.xml | 1 + capabilities2_server/CMakeLists.txt | 3 + .../capabilities2_server/capabilities_api.hpp | 4 +- .../capabilities_server.hpp | 13 ++- .../capabilities2_server/runner_cache.hpp | 2 +- capabilities2_server/package.xml | 1 + .../capabilities2_utils/connection.hpp | 1 + .../capabilities2_utils/event_types.hpp | 47 +++++++++++ 27 files changed, 209 insertions(+), 182 deletions(-) create mode 100644 capabilities2_utils/include/capabilities2_utils/event_types.hpp diff --git a/capabilities2_msgs/srv/ConfigureCapability.srv b/capabilities2_msgs/srv/ConfigureCapability.srv index 726dab7..b9e6905 100644 --- a/capabilities2_msgs/srv/ConfigureCapability.srv +++ b/capabilities2_msgs/srv/ConfigureCapability.srv @@ -4,4 +4,5 @@ Capability target_on_stop Capability target_on_success Capability target_on_failure string connection_description +int32 trigger_id --- \ No newline at end of file diff --git a/capabilities2_runner/CMakeLists.txt b/capabilities2_runner/CMakeLists.txt index 74610f5..6021b30 100644 --- a/capabilities2_runner/CMakeLists.txt +++ b/capabilities2_runner/CMakeLists.txt @@ -15,6 +15,7 @@ find_package(rclcpp REQUIRED) find_package(rclcpp_action REQUIRED) find_package(pluginlib REQUIRED) find_package(capabilities2_msgs REQUIRED) +find_package(capabilities2_utils REQUIRED) find_package(event_logger REQUIRED) find_package(event_logger_msgs REQUIRED) find_package(tinyxml2_vendor REQUIRED) @@ -33,6 +34,7 @@ ament_target_dependencies(${PROJECT_NAME} rclcpp_action pluginlib capabilities2_msgs + capabilities2_utils event_logger event_logger_msgs TinyXML2 diff --git a/capabilities2_runner/include/capabilities2_runner/action_runner.hpp b/capabilities2_runner/include/capabilities2_runner/action_runner.hpp index f4c88c8..de2786a 100644 --- a/capabilities2_runner/include/capabilities2_runner/action_runner.hpp +++ b/capabilities2_runner/include/capabilities2_runner/action_runner.hpp @@ -91,12 +91,11 @@ class ActionRunner : public RunnerBase } // Trigger on_stopped event if defined - if (events[execute_id].on_stopped.interface != "") + if (events[runner_id].on_stopped.interface != "") { - event_(EventType::STOPPED, -1, events[execute_id].on_stopped.interface, - events[execute_id].on_stopped.provider); - triggerFunction_(events[execute_id].on_stopped.interface, - update_on_stopped(events[execute_id].on_stopped.parameters)); + event_(EventType::STOPPED, -1, events[runner_id].on_stopped.interface, events[runner_id].on_stopped.provider); + triggerFunction_(events[runner_id].on_stopped.interface, + update_on_stopped(events[runner_id].on_stopped.parameters)); } }); @@ -129,8 +128,6 @@ class ActionRunner : public RunnerBase */ virtual void execution(int id) override { - execute_id += 1; - // if parameters are not provided then cannot proceed if (!parameters_[id]) throw runner_exception("cannot trigger action without parameters"); @@ -138,7 +135,7 @@ class ActionRunner : public RunnerBase // generate a goal from parameters if provided goal_msg_ = generate_goal(parameters_[id], id); - info_("goal generated", id); + info_("goal generated for event ", id); std::unique_lock lock(mutex_); completed_ = false; @@ -151,12 +148,10 @@ class ActionRunner : public RunnerBase info_("goal accepted. Waiting for result", id); // trigger the events related to on_started state - if (events[execute_id].on_started.interface != "") + if (events[id].on_started.interface != "") { - event_(EventType::STARTED, id, events[execute_id].on_started.interface, - events[execute_id].on_started.provider); - triggerFunction_(events[execute_id].on_started.interface, - update_on_started(events[execute_id].on_started.parameters)); + event_(EventType::STARTED, id, events[id].on_started.interface, events[id].on_started.provider); + triggerFunction_(events[id].on_started.interface, update_on_started(events[id].on_started.parameters)); } } else @@ -187,12 +182,10 @@ class ActionRunner : public RunnerBase info_("action succeeded.", id); // trigger the events related to on_success state - if (events[execute_id].on_success.interface != "") + if (events[id].on_success.interface != "") { - event_(EventType::SUCCEEDED, id, events[execute_id].on_success.interface, - events[execute_id].on_success.provider); - triggerFunction_(events[execute_id].on_success.interface, - update_on_success(events[execute_id].on_success.parameters)); + event_(EventType::SUCCEEDED, id, events[id].on_success.interface, events[id].on_success.provider); + triggerFunction_(events[id].on_success.interface, update_on_success(events[id].on_success.parameters)); } } else @@ -200,12 +193,10 @@ class ActionRunner : public RunnerBase error_("action failed", id); // trigger the events related to on_failure state - if (events[execute_id].on_failure.interface != "") + if (events[id].on_failure.interface != "") { - event_(EventType::FAILED, id, events[execute_id].on_failure.interface, - events[execute_id].on_failure.provider); - triggerFunction_(events[execute_id].on_failure.interface, - update_on_failure(events[execute_id].on_failure.parameters)); + event_(EventType::FAILED, id, events[id].on_failure.interface, events[id].on_failure.provider); + triggerFunction_(events[id].on_failure.interface, update_on_failure(events[id].on_failure.parameters)); } } diff --git a/capabilities2_runner/include/capabilities2_runner/runner_base.hpp b/capabilities2_runner/include/capabilities2_runner/runner_base.hpp index a9115bf..65d8659 100644 --- a/capabilities2_runner/include/capabilities2_runner/runner_base.hpp +++ b/capabilities2_runner/include/capabilities2_runner/runner_base.hpp @@ -7,9 +7,11 @@ #include #include #include + +#include + #include #include -#include #include namespace capabilities2_runner @@ -68,7 +70,7 @@ class RunnerBase { public: using Event = event_logger_msgs::msg::Event; - using EventType = event_logger::event_t; + using EventType = capabilities2::event_t; RunnerBase() : run_config_() { @@ -106,15 +108,18 @@ class RunnerBase */ virtual void trigger(const std::string& parameters) { - info_("received new parameters", thread_id); + // extract the unique id for the runner and use that as the thread id + tinyxml2::XMLElement * element = nullptr; + element = convert_to_xml(parameters); + element->QueryIntAttribute("id", &runner_id); - parameters_[thread_id] = convert_to_xml(parameters); + parameters_[runner_id] = element; - executionThreadPool[thread_id] = std::thread(&RunnerBase::execution, this, thread_id); + info_("received new parameters with event id", runner_id); - info_("started execution", thread_id); + executionThreadPool[runner_id] = std::thread(&RunnerBase::execution, this, runner_id); - thread_id += 1; + info_("started execution", runner_id); } /** @@ -129,10 +134,8 @@ class RunnerBase node_ = node; run_config_ = run_config; - insert_id = 0; - execute_id = -1; - thread_id = 0; current_inputs_ = 0; + runner_id = 0; event_client_ = std::make_shared(node_, "runner", "/events"); } @@ -145,17 +148,14 @@ class RunnerBase * * @return number of attached events */ - virtual int attach_events(event_logger::event_opts& event_option, + virtual void attach_events(capabilities2::event_opts& event_option, std::function triggerFunction) { - info_("accepted event options with ID : " + std::to_string(insert_id)); + info_("accepted event options with ID : " + std::to_string(event_option.event_id)); triggerFunction_ = triggerFunction; - events[insert_id] = event_option; - insert_id += 1; - - return insert_id; + events[event_option.event_id] = event_option; } /** @@ -213,8 +213,9 @@ class RunnerBase * @brief Trigger process to be executed. * * This method utilizes paramters set via the trigger() function - * - * @param parameters pointer to tinyxml2::XMLElement that contains parameters + * + * @param id unique identifier for the runner id. used to track the correct + * triggers and subsequent events. * */ virtual void execution(int id) = 0; @@ -571,22 +572,12 @@ class RunnerBase /** * @brief dictionary of events */ - std::map events; - - /** - * @brief Last event tracker id to be inserted - */ - int insert_id; - - /** - * @brief Last parameter tracker id to be executed - */ - int execute_id; + std::map events; /** - * @brief Last parameter tracker id to be executed + * @brief unique id for the runner */ - int thread_id; + int runner_id; /** * @brief curent number of trigger signals received diff --git a/capabilities2_runner/include/capabilities2_runner/service_runner.hpp b/capabilities2_runner/include/capabilities2_runner/service_runner.hpp index d0017ef..5238c0b 100644 --- a/capabilities2_runner/include/capabilities2_runner/service_runner.hpp +++ b/capabilities2_runner/include/capabilities2_runner/service_runner.hpp @@ -61,8 +61,6 @@ class ServiceRunner : public RunnerBase */ virtual void execution(int id) override { - execute_id += 1; - // if parameters are not provided then cannot proceed if (!parameters_[id]) throw runner_exception("cannot trigger service without parameters"); @@ -70,7 +68,7 @@ class ServiceRunner : public RunnerBase // generate a goal from parameters if provided auto request_msg = std::make_shared(generate_request(parameters_[id], id)); - info_("request generated", id); + info_("request generated for event :", id); std::unique_lock lock(mutex_); completed_ = false; @@ -82,12 +80,10 @@ class ServiceRunner : public RunnerBase error_("get result call failed"); // trigger the events related to on_failure state - if (events[execute_id].on_failure.interface != "") + if (events[id].on_failure.interface != "") { - event_(EventType::FAILED, id, events[execute_id].on_failure.interface, - events[execute_id].on_failure.provider); - triggerFunction_(events[execute_id].on_failure.interface, - update_on_failure(events[execute_id].on_failure.parameters)); + event_(EventType::FAILED, id, events[id].on_failure.interface, events[id].on_failure.provider); + triggerFunction_(events[id].on_failure.interface, update_on_failure(events[id].on_failure.parameters)); } } else @@ -98,12 +94,10 @@ class ServiceRunner : public RunnerBase process_response(response_, id); // trigger the events related to on_success state - if (events[execute_id].on_success.interface != "") + if (events[id].on_success.interface != "") { - event_(EventType::SUCCEEDED, id, events[execute_id].on_success.interface, - events[execute_id].on_success.provider); - triggerFunction_(events[execute_id].on_success.interface, - update_on_success(events[execute_id].on_success.parameters)); + event_(EventType::SUCCEEDED, id, events[id].on_success.interface, events[id].on_success.provider); + triggerFunction_(events[id].on_success.interface, update_on_success(events[id].on_success.parameters)); } } @@ -112,11 +106,10 @@ class ServiceRunner : public RunnerBase }); // trigger the events related to on_started state - if (events[execute_id].on_started.interface != "") + if (events[id].on_started.interface != "") { - event_(EventType::STARTED, id, events[execute_id].on_started.interface, events[execute_id].on_started.provider); - triggerFunction_(events[execute_id].on_started.interface, - update_on_started(events[execute_id].on_started.parameters)); + event_(EventType::STARTED, id, events[id].on_started.interface, events[id].on_started.provider); + triggerFunction_(events[id].on_started.interface, update_on_started(events[id].on_started.parameters)); } // Conditional wait @@ -143,11 +136,11 @@ class ServiceRunner : public RunnerBase throw runner_exception("cannot stop runner action that was not started"); // Trigger on_stopped event if defined - if (events[execute_id].on_stopped.interface != "") + if (events[runner_id].on_stopped.interface != "") { - event_(EventType::STOPPED, -1, events[execute_id].on_stopped.interface, events[execute_id].on_stopped.provider); - triggerFunction_(events[execute_id].on_stopped.interface, - update_on_stopped(events[execute_id].on_stopped.parameters)); + event_(EventType::STOPPED, -1, events[runner_id].on_stopped.interface, events[runner_id].on_stopped.provider); + triggerFunction_(events[runner_id].on_stopped.interface, + update_on_stopped(events[runner_id].on_stopped.parameters)); } info_("stopping runner"); diff --git a/capabilities2_runner/include/capabilities2_runner/topic_runner.hpp b/capabilities2_runner/include/capabilities2_runner/topic_runner.hpp index f2a6893..0fb84ce 100644 --- a/capabilities2_runner/include/capabilities2_runner/topic_runner.hpp +++ b/capabilities2_runner/include/capabilities2_runner/topic_runner.hpp @@ -51,18 +51,15 @@ class TopicRunner : public RunnerBase */ virtual void execution(int id) override { - execute_id += 1; - // if parameters are not provided then cannot proceed if (!parameters_[id]) throw runner_exception("cannot grab data without parameters"); // trigger the events related to on_started state - if (events[execute_id].on_started.interface != "") + if (events[id].on_started.interface != "") { - event_(EventType::STARTED, id, events[execute_id].on_started.interface, events[execute_id].on_started.provider); - triggerFunction_(events[execute_id].on_started.interface, - update_on_started(events[execute_id].on_started.parameters)); + event_(EventType::STARTED, id, events[id].on_started.interface, events[id].on_started.provider); + triggerFunction_(events[id].on_started.interface, update_on_started(events[id].on_started.parameters)); } std::unique_lock lock(mutex_); @@ -77,12 +74,10 @@ class TopicRunner : public RunnerBase if (latest_message_) { // trigger the events related to on_success state - if (events[execute_id].on_success.interface != "") + if (events[id].on_success.interface != "") { - event_(EventType::SUCCEEDED, id, events[execute_id].on_success.interface, - events[execute_id].on_success.provider); - triggerFunction_(events[execute_id].on_success.interface, - update_on_success(events[execute_id].on_success.parameters)); + event_(EventType::SUCCEEDED, id, events[id].on_success.interface, events[id].on_success.provider); + triggerFunction_(events[id].on_success.interface, update_on_success(events[id].on_success.parameters)); } } else @@ -90,11 +85,10 @@ class TopicRunner : public RunnerBase error_("Message receving failed."); // trigger the events related to on_failure state - if (events[execute_id].on_failure.interface != "") + if (events[id].on_failure.interface != "") { - event_(EventType::FAILED, id, events[execute_id].on_failure.interface, events[execute_id].on_failure.provider); - triggerFunction_(events[execute_id].on_failure.interface, - update_on_failure(events[execute_id].on_failure.parameters)); + event_(EventType::FAILED, id, events[id].on_failure.interface, events[id].on_failure.provider); + triggerFunction_(events[id].on_failure.interface, update_on_failure(events[id].on_failure.parameters)); } } @@ -120,11 +114,11 @@ class TopicRunner : public RunnerBase throw runner_exception("cannot stop runner subscriber that was not started"); // Trigger on_stopped event if defined - if (events[execute_id].on_stopped.interface != "") + if (events[runner_id].on_stopped.interface != "") { - event_(EventType::STOPPED, -1, events[execute_id].on_stopped.interface, events[execute_id].on_stopped.provider); - triggerFunction_(events[execute_id].on_stopped.interface, - update_on_stopped(events[execute_id].on_stopped.parameters)); + event_(EventType::STOPPED, -1, events[runner_id].on_stopped.interface, events[runner_id].on_stopped.provider); + triggerFunction_(events[runner_id].on_stopped.interface, + update_on_stopped(events[runner_id].on_stopped.parameters)); } info_("stopping runner"); diff --git a/capabilities2_runner/package.xml b/capabilities2_runner/package.xml index 248120c..f33f924 100644 --- a/capabilities2_runner/package.xml +++ b/capabilities2_runner/package.xml @@ -21,6 +21,7 @@ pluginlib std_msgs capabilities2_msgs + capabilities2_utils event_logger event_logger_msgs tinyxml2_vendor diff --git a/capabilities2_runner_audio/CMakeLists.txt b/capabilities2_runner_audio/CMakeLists.txt index 14c53d0..c03dd00 100644 --- a/capabilities2_runner_audio/CMakeLists.txt +++ b/capabilities2_runner_audio/CMakeLists.txt @@ -18,6 +18,7 @@ find_package(pluginlib REQUIRED) find_package(hri_audio_msgs REQUIRED) find_package(capabilities2_msgs REQUIRED) find_package(capabilities2_runner REQUIRED) +find_package(capabilities2_utils REQUIRED) find_package(event_logger REQUIRED) find_package(event_logger_msgs REQUIRED) find_package(tinyxml2_vendor REQUIRED) @@ -38,6 +39,7 @@ ament_target_dependencies(${PROJECT_NAME} hri_audio_msgs capabilities2_msgs capabilities2_runner + capabilities2_utils event_logger event_logger_msgs TinyXML2 diff --git a/capabilities2_runner_audio/package.xml b/capabilities2_runner_audio/package.xml index 787289b..edccb72 100644 --- a/capabilities2_runner_audio/package.xml +++ b/capabilities2_runner_audio/package.xml @@ -21,6 +21,7 @@ hri_audio_msgs capabilities2_msgs capabilities2_runner + capabilities2_utils event_logger event_logger_msgs tinyxml2_vendor diff --git a/capabilities2_runner_bt/CMakeLists.txt b/capabilities2_runner_bt/CMakeLists.txt index 1052e22..25bace3 100644 --- a/capabilities2_runner_bt/CMakeLists.txt +++ b/capabilities2_runner_bt/CMakeLists.txt @@ -16,6 +16,7 @@ find_package(rclcpp_action REQUIRED) find_package(pluginlib REQUIRED) find_package(capabilities2_msgs REQUIRED) find_package(capabilities2_runner REQUIRED) +find_package(capabilities2_utils REQUIRED) find_package(event_logger REQUIRED) find_package(event_logger_msgs REQUIRED) find_package(behaviortree_cpp REQUIRED) @@ -36,6 +37,7 @@ ament_target_dependencies(${PROJECT_NAME} pluginlib capabilities2_msgs capabilities2_runner + capabilities2_utils event_logger event_logger_msgs behaviortree_cpp diff --git a/capabilities2_runner_bt/package.xml b/capabilities2_runner_bt/package.xml index d41e1c7..07668d9 100644 --- a/capabilities2_runner_bt/package.xml +++ b/capabilities2_runner_bt/package.xml @@ -21,6 +21,7 @@ std_msgs capabilities2_msgs capabilities2_runner + capabilities2_utils event_logger event_logger_msgs tinyxml2_vendor diff --git a/capabilities2_runner_capabilities/CMakeLists.txt b/capabilities2_runner_capabilities/CMakeLists.txt index 57153c2..6dc43ee 100644 --- a/capabilities2_runner_capabilities/CMakeLists.txt +++ b/capabilities2_runner_capabilities/CMakeLists.txt @@ -16,6 +16,7 @@ find_package(rclcpp_action REQUIRED) find_package(pluginlib REQUIRED) find_package(capabilities2_msgs REQUIRED) find_package(capabilities2_runner REQUIRED) +find_package(capabilities2_utils REQUIRED) find_package(event_logger REQUIRED) find_package(event_logger_msgs REQUIRED) find_package(tinyxml2_vendor REQUIRED) @@ -35,6 +36,7 @@ ament_target_dependencies(${PROJECT_NAME} pluginlib capabilities2_msgs capabilities2_runner + capabilities2_utils event_logger event_logger_msgs TinyXML2 diff --git a/capabilities2_runner_fabric/CMakeLists.txt b/capabilities2_runner_fabric/CMakeLists.txt index 15dc76e..b813db7 100644 --- a/capabilities2_runner_fabric/CMakeLists.txt +++ b/capabilities2_runner_fabric/CMakeLists.txt @@ -17,6 +17,7 @@ find_package(pluginlib REQUIRED) find_package(fabric_msgs REQUIRED) find_package(capabilities2_msgs REQUIRED) find_package(capabilities2_runner REQUIRED) +find_package(capabilities2_utils REQUIRED) find_package(event_logger REQUIRED) find_package(event_logger_msgs REQUIRED) find_package(tinyxml2_vendor REQUIRED) @@ -37,6 +38,7 @@ ament_target_dependencies(${PROJECT_NAME} fabric_msgs capabilities2_msgs capabilities2_runner + capabilities2_utils event_logger event_logger_msgs TinyXML2 diff --git a/capabilities2_runner_nav2/CMakeLists.txt b/capabilities2_runner_nav2/CMakeLists.txt index 61d91a3..01483a7 100644 --- a/capabilities2_runner_nav2/CMakeLists.txt +++ b/capabilities2_runner_nav2/CMakeLists.txt @@ -20,6 +20,7 @@ find_package(tf2_ros REQUIRED) find_package(geometry_msgs REQUIRED) find_package(capabilities2_msgs REQUIRED) find_package(capabilities2_runner REQUIRED) +find_package(capabilities2_utils REQUIRED) find_package(event_logger REQUIRED) find_package(event_logger_msgs REQUIRED) find_package(tinyxml2_vendor REQUIRED) @@ -43,6 +44,7 @@ ament_target_dependencies(${PROJECT_NAME} geometry_msgs capabilities2_msgs capabilities2_runner + capabilities2_utils event_logger event_logger_msgs TinyXML2 diff --git a/capabilities2_runner_prompt/CMakeLists.txt b/capabilities2_runner_prompt/CMakeLists.txt index b0a509c..8df7df5 100644 --- a/capabilities2_runner_prompt/CMakeLists.txt +++ b/capabilities2_runner_prompt/CMakeLists.txt @@ -17,6 +17,7 @@ find_package(pluginlib REQUIRED) find_package(prompt_msgs REQUIRED) find_package(capabilities2_msgs REQUIRED) find_package(capabilities2_runner REQUIRED) +find_package(capabilities2_utils REQUIRED) find_package(event_logger REQUIRED) find_package(event_logger_msgs REQUIRED) find_package(tinyxml2_vendor REQUIRED) @@ -37,6 +38,7 @@ ament_target_dependencies(${PROJECT_NAME} prompt_msgs capabilities2_msgs capabilities2_runner + capabilities2_utils event_logger event_logger_msgs TinyXML2 diff --git a/capabilities2_runner_system/CMakeLists.txt b/capabilities2_runner_system/CMakeLists.txt index 6f4d9c3..9d56357 100644 --- a/capabilities2_runner_system/CMakeLists.txt +++ b/capabilities2_runner_system/CMakeLists.txt @@ -16,6 +16,7 @@ find_package(rclcpp_action REQUIRED) find_package(pluginlib REQUIRED) find_package(capabilities2_msgs REQUIRED) find_package(capabilities2_runner REQUIRED) +find_package(capabilities2_utils REQUIRED) find_package(fabric_msgs REQUIRED) find_package(event_logger REQUIRED) find_package(event_logger_msgs REQUIRED) @@ -37,6 +38,7 @@ ament_target_dependencies(${PROJECT_NAME} capabilities2_msgs fabric_msgs capabilities2_runner + capabilities2_utils event_logger event_logger_msgs TinyXML2 diff --git a/capabilities2_runner_system/include/capabilities2_runner_system/input_multiplex_all_runner.hpp b/capabilities2_runner_system/include/capabilities2_runner_system/input_multiplex_all_runner.hpp index 6def71a..5bc8cac 100644 --- a/capabilities2_runner_system/include/capabilities2_runner_system/input_multiplex_all_runner.hpp +++ b/capabilities2_runner_system/include/capabilities2_runner_system/input_multiplex_all_runner.hpp @@ -23,35 +23,33 @@ class InputMultiplexAllRunner : public MultiplexBaseRunner { tinyxml2::XMLElement* parameters_ = convert_to_xml(parameters); - int uid = 0; int input_count = 0; parameters_->QueryIntAttribute("input_count", &input_count); - parameters_->QueryIntAttribute("uid", &uid); + parameters_->QueryIntAttribute("id", &runner_id); - if (input_count_tracker.find(uid) == input_count_tracker.end()) + if (input_count_tracker.find(runner_id) == input_count_tracker.end()) { - input_count_tracker[uid] = 1; - expected_input_count[uid] = input_count; + input_count_tracker[runner_id] = 1; + expected_input_count[runner_id] = input_count; - info_("has started the All condition with " + std::to_string(input_count_tracker[uid]) + " inputs."); + info_("has started the All condition with " + std::to_string(input_count_tracker[runner_id]) + " inputs."); } else { - input_count_tracker[uid] += 1; + input_count_tracker[runner_id] += 1; - info_("has received " + std::to_string(input_count_tracker[uid]) + "/" + - std::to_string(expected_input_count[uid]) + " inputs for ALL condition."); + info_("has received " + std::to_string(input_count_tracker[runner_id]) + "/" + + std::to_string(expected_input_count[runner_id]) + " inputs for ALL condition."); } - if (input_count_tracker[uid] == expected_input_count[uid]) + if (input_count_tracker[runner_id] == expected_input_count[runner_id]) { - info_("has fullfilled the All condition with " + std::to_string(input_count_tracker[uid]) + " inputs."); + info_("has fullfilled the All condition with " + std::to_string(input_count_tracker[runner_id]) + " inputs."); - executionThreadPool[uid] = std::thread(&InputMultiplexAllRunner::execution, this, uid); + executionThreadPool[runner_id] = std::thread(&InputMultiplexAllRunner::execution, this, runner_id); } } - }; } // namespace capabilities2_runner diff --git a/capabilities2_runner_system/include/capabilities2_runner_system/input_multiplex_any_runner.hpp b/capabilities2_runner_system/include/capabilities2_runner_system/input_multiplex_any_runner.hpp index 5960a77..dfc919f 100644 --- a/capabilities2_runner_system/include/capabilities2_runner_system/input_multiplex_any_runner.hpp +++ b/capabilities2_runner_system/include/capabilities2_runner_system/input_multiplex_any_runner.hpp @@ -21,37 +21,46 @@ class InputMultiplexAnyRunner : public MultiplexBaseRunner */ virtual void trigger(const std::string& parameters) override { + info_("received new parameters for InputMultiplexAnyRunner : " + parameters); + tinyxml2::XMLElement* parameters_ = convert_to_xml(parameters); - int uid = 0; - int input_count = 0; + int input_count = -1; parameters_->QueryIntAttribute("input_count", &input_count); - parameters_->QueryIntAttribute("uid", &uid); + parameters_->QueryIntAttribute("id", &runner_id); + + if (runner_id < 0 || input_count < 0) + { + throw runner_exception("UID or input_count not found in parameters"); + } + else + { + info_("triggered with UID: " + std::to_string(runner_id) + " and input_count: " + std::to_string(input_count)); + } - if (input_count_tracker.find(uid) == input_count_tracker.end()) + if (input_count_tracker.find(runner_id) == input_count_tracker.end()) { - input_count_tracker[uid] = 1; - expected_input_count[uid] = input_count; + input_count_tracker[runner_id] = 1; + expected_input_count[runner_id] = input_count; - info_("has started the ANY condition with " + std::to_string(input_count_tracker[uid]) + " inputs."); + info_("has started the ANY condition with " + std::to_string(input_count_tracker[runner_id]) + " inputs."); } else { - input_count_tracker[uid] += 1; + input_count_tracker[runner_id] += 1; - info_("has received " + std::to_string(input_count_tracker[uid]) + "/" + - std::to_string(expected_input_count[uid]) + " inputs for ANY condition."); + info_("has received " + std::to_string(input_count_tracker[runner_id]) + "/" + + std::to_string(expected_input_count[runner_id]) + " inputs for ANY condition."); } - if (input_count_tracker[uid] > 0) + if (input_count_tracker[runner_id] > 0) { - info_("has fullfilled the ANY condition with " + std::to_string(input_count_tracker[uid]) + " inputs."); + info_("has fullfilled the ANY condition with " + std::to_string(input_count_tracker[runner_id]) + " inputs."); - executionThreadPool[uid] = std::thread(&InputMultiplexAnyRunner::execution, this, uid); + executionThreadPool[runner_id] = std::thread(&InputMultiplexAnyRunner::execution, this, runner_id); } } - }; } // namespace capabilities2_runner diff --git a/capabilities2_runner_system/include/capabilities2_runner_system/multiplex_base_runner.hpp b/capabilities2_runner_system/include/capabilities2_runner_system/multiplex_base_runner.hpp index c02739d..10d2c61 100644 --- a/capabilities2_runner_system/include/capabilities2_runner_system/multiplex_base_runner.hpp +++ b/capabilities2_runner_system/include/capabilities2_runner_system/multiplex_base_runner.hpp @@ -31,70 +31,38 @@ class MultiplexBaseRunner : public RunnerBase /** * @brief Trigger process to be executed. * - * @param uid unique identifier for the execution + * @param id unique identifier for the execution */ - virtual void execution(int uid) + virtual void execution(int id) { - info_("execution started for uid: " + std::to_string(uid)); + info_("execution started for id: " + std::to_string(id)); - // trigger the events related to on_success state - if (events[uid].on_success.interface != "") + // check if the id is already completed + if (completed_executions.find(id) != completed_executions.end() && completed_executions[id]) { - event_(EventType::SUCCEEDED, uid, events[uid].on_success.interface, events[uid].on_success.provider); - triggerFunction_(events[uid].on_success.interface, update_on_success(events[uid].on_success.parameters)); + info_("execution already completed for id: " + std::to_string(id)); + return; } - // trigger the events related to on_failure state - else if (events[uid].on_failure.interface != "") + else { - event_(EventType::FAILED, uid, events[uid].on_failure.interface, events[uid].on_failure.provider); - triggerFunction_(events[uid].on_failure.interface, update_on_failure(events[uid].on_failure.parameters)); + // trigger the events related to on_success state + if (events[id].on_success.interface != "") + { + event_(EventType::SUCCEEDED, id, events[id].on_success.interface, events[id].on_success.provider); + triggerFunction_(events[id].on_success.interface, update_on_success(events[id].on_success.parameters)); + } + // trigger the events related to on_failure state + else if (events[id].on_failure.interface != "") + { + event_(EventType::FAILED, id, events[id].on_failure.interface, events[id].on_failure.provider); + triggerFunction_(events[id].on_failure.interface, update_on_failure(events[id].on_failure.parameters)); + } } - } - - /** - * @brief attach events to the runner - * - * @param event_option event_options related for the action - * @param triggerFunction external function that triggers capability runners - * - * @return number of attached events - */ - virtual int attach_events(event_logger::event_opts& event_option, - std::function triggerFunction) override - { - info_("accepted event options with ID : " + std::to_string(insert_id)); - - triggerFunction_ = triggerFunction; - - tinyxml2::XMLElement* on_success_params = convert_to_xml(event_option.on_success.parameters); - - int uid = NULL; - // extract the uid from the event options from whatever runner is present by looping - if (event_option.on_success.interface != "") - { - tinyxml2::XMLElement* on_success_params = convert_to_xml(event_option.on_success.parameters); - on_success_params->QueryIntAttribute("uid", &uid); - } - else if (event_option.on_failure.interface != "") - { - tinyxml2::XMLElement* on_failure_params = convert_to_xml(event_option.on_failure.parameters); - on_failure_params->QueryIntAttribute("uid", &uid); - } - else if (event_option.on_started.interface != "") - { - tinyxml2::XMLElement* on_started_params = convert_to_xml(event_option.on_started.parameters); - on_started_params->QueryIntAttribute("uid", &uid); - } - else if (event_option.on_stopped.interface != "") - { - tinyxml2::XMLElement* on_stopped_params = convert_to_xml(event_option.on_stopped.parameters); - on_stopped_params->QueryIntAttribute("uid", &uid); - } + // track the execution as completed + completed_executions[id] = true; - events[uid] = event_option; - - return uid; + info_("multiplexing complete. Thread closing.", id); } /** @@ -118,5 +86,8 @@ class MultiplexBaseRunner : public RunnerBase // expected input count std::map expected_input_count; + + // completed executions + std::map completed_executions; }; } // namespace capabilities2_runner \ No newline at end of file diff --git a/capabilities2_runner_system/package.xml b/capabilities2_runner_system/package.xml index 2f3f807..dd79d51 100644 --- a/capabilities2_runner_system/package.xml +++ b/capabilities2_runner_system/package.xml @@ -24,6 +24,7 @@ std_msgs capabilities2_msgs capabilities2_runner + capabilities2_utils fabric_msgs event_logger event_logger_msgs diff --git a/capabilities2_server/CMakeLists.txt b/capabilities2_server/CMakeLists.txt index 0088818..bc9f429 100644 --- a/capabilities2_server/CMakeLists.txt +++ b/capabilities2_server/CMakeLists.txt @@ -19,6 +19,7 @@ find_package(bondcpp REQUIRED) find_package(pluginlib REQUIRED) find_package(capabilities2_msgs REQUIRED) find_package(capabilities2_runner REQUIRED) +find_package(capabilities2_utils REQUIRED) find_package(event_logger REQUIRED) find_package(event_logger_msgs REQUIRED) find_package(tinyxml2_vendor REQUIRED) @@ -62,6 +63,7 @@ ament_target_dependencies(${PROJECT_NAME}_comp rclcpp_components capabilities2_msgs capabilities2_runner + capabilities2_utils event_logger event_logger_msgs TinyXML2 @@ -105,6 +107,7 @@ ament_target_dependencies(${PROJECT_NAME}_node pluginlib capabilities2_msgs capabilities2_runner + capabilities2_utils event_logger event_logger_msgs TinyXML2 diff --git a/capabilities2_server/include/capabilities2_server/capabilities_api.hpp b/capabilities2_server/include/capabilities2_server/capabilities_api.hpp index 97b8b24..e7a246a 100644 --- a/capabilities2_server/include/capabilities2_server/capabilities_api.hpp +++ b/capabilities2_server/include/capabilities2_server/capabilities_api.hpp @@ -16,8 +16,8 @@ #include #include #include +#include -#include #include #include @@ -241,7 +241,7 @@ class CapabilitiesAPI * @param capability capability from where the events originate * @param event_options event options for the capability */ - void set_triggers(const std::string& capability, event_logger::event_opts& event_options) + void set_triggers(const std::string& capability, capabilities2::event_opts& event_options) { try { diff --git a/capabilities2_server/include/capabilities2_server/capabilities_server.hpp b/capabilities2_server/include/capabilities2_server/capabilities_server.hpp index 32528b6..e5e5b30 100644 --- a/capabilities2_server/include/capabilities2_server/capabilities_server.hpp +++ b/capabilities2_server/include/capabilities2_server/capabilities_server.hpp @@ -16,7 +16,6 @@ #include #include - #include #include #include @@ -33,8 +32,9 @@ #include #include +#include + #include -#include #include namespace capabilities2_server @@ -318,7 +318,9 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI void configure_capability_cb(const std::shared_ptr req, std::shared_ptr res) { - event_logger::event_opts event_options; + capabilities2::event_opts event_options; + + event_options.event_id = req->trigger_id; event_options.on_started.interface = req->target_on_start.capability; event_options.on_started.provider = req->target_on_start.provider; @@ -348,6 +350,11 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI event_->runner_define(req->source.capability, req->source.provider, req->target_on_stop.capability, req->target_on_stop.provider, event_logger_msgs::msg::Event::STOPPED, req->connection_description); + event_->info("on_started : " + event_options.on_started.parameters); + event_->info("on_failure : " + event_options.on_failure.parameters); + event_->info("on_success : " + event_options.on_success.parameters); + event_->info("on_stopped : " + event_options.on_stopped.parameters); + // setup triggers between parameters set_triggers(req->source.capability, event_options); diff --git a/capabilities2_server/include/capabilities2_server/runner_cache.hpp b/capabilities2_server/include/capabilities2_server/runner_cache.hpp index 4c1aac1..db7c9fe 100644 --- a/capabilities2_server/include/capabilities2_server/runner_cache.hpp +++ b/capabilities2_server/include/capabilities2_server/runner_cache.hpp @@ -112,7 +112,7 @@ class RunnerCache * @param on_success on_success event with capability and parameters * @param on_stopped on_stop event with capability and parameters */ - void set_runner_triggers(const std::string& capability, event_logger::event_opts& event_options) + void set_runner_triggers(const std::string& capability, capabilities2::event_opts& event_options) { runner_cache_[capability]->attach_events(event_options, std::bind(&capabilities2_server::RunnerCache::trigger_runner, this, diff --git a/capabilities2_server/package.xml b/capabilities2_server/package.xml index 0a8ab8a..3efffc8 100644 --- a/capabilities2_server/package.xml +++ b/capabilities2_server/package.xml @@ -23,6 +23,7 @@ rclcpp_components capabilities2_msgs capabilities2_runner + capabilities2_utils event_logger event_logger_msgs tinyxml2_vendor diff --git a/capabilities2_utils/include/capabilities2_utils/connection.hpp b/capabilities2_utils/include/capabilities2_utils/connection.hpp index c88ede3..4d8acca 100644 --- a/capabilities2_utils/include/capabilities2_utils/connection.hpp +++ b/capabilities2_utils/include/capabilities2_utils/connection.hpp @@ -26,6 +26,7 @@ namespace capabilities2 connection_t target_on_success; connection_t target_on_failure; std::string connection_description; + int trigger_id = -1; }; } // namespace capabilities2 diff --git a/capabilities2_utils/include/capabilities2_utils/event_types.hpp b/capabilities2_utils/include/capabilities2_utils/event_types.hpp new file mode 100644 index 0000000..9f756ef --- /dev/null +++ b/capabilities2_utils/include/capabilities2_utils/event_types.hpp @@ -0,0 +1,47 @@ +#pragma once +#include + +namespace capabilities2 +{ +enum event_t +{ + IDLE, + STARTED, + STOPPED, + FAILED, + SUCCEEDED +}; + +/** + * @brief event definition + * + * Contains the informations about the event to be executed. It contains the interface, provider and parameters + */ +struct event +{ + std::string interface; + std::string provider; + std::string parameters; +}; + +/** + * @brief event options + * + * keeps track of events that are related to runner instances at various points of the + * plan + * @param event_id unique identifier for the event + * @param on_started information about the capability to execute on start + * @param on_success information about the capability to execute on success + * @param on_failure information about the capability to execute on failure + * @param on_stopped information about the capability to execute on stop + */ +struct event_opts +{ + int event_id = -1; + event on_started; + event on_success; + event on_failure; + event on_stopped; +}; + +} \ No newline at end of file From 6a448731a0480915d5ed1e174337644aa174bcc9 Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Sun, 17 Aug 2025 20:46:10 +1000 Subject: [PATCH 125/133] redundant parameter cleaning --- .../robotpose_runner.hpp | 41 ++++++++----------- 1 file changed, 16 insertions(+), 25 deletions(-) diff --git a/capabilities2_runner_nav2/include/capabilities2_runner_nav2/robotpose_runner.hpp b/capabilities2_runner_nav2/include/capabilities2_runner_nav2/robotpose_runner.hpp index 5ce0429..4f7d8bd 100644 --- a/capabilities2_runner_nav2/include/capabilities2_runner_nav2/robotpose_runner.hpp +++ b/capabilities2_runner_nav2/include/capabilities2_runner_nav2/robotpose_runner.hpp @@ -55,18 +55,15 @@ class RobotPoseRunner : public RunnerBase const char* odom; const char* robot; - execute_id += 1; - // if parameters are not provided then cannot proceed if (!parameters_[id]) throw runner_exception("cannot grab data without parameters"); // trigger the events related to on_started state - if (events[execute_id].on_started.interface != "") + if (events[id].on_started.interface != "") { - event_(EventType::STARTED, id, events[execute_id].on_started.interface, events[execute_id].on_started.provider); - triggerFunction_(events[execute_id].on_started.interface, - update_on_started(events[execute_id].on_started.parameters)); + event_(EventType::STARTED, id, events[id].on_started.interface, events[id].on_started.provider); + triggerFunction_(events[id].on_started.interface, update_on_started(events[id].on_started.parameters)); } info_("Waiting for Transformation.", id); @@ -87,12 +84,10 @@ class RobotPoseRunner : public RunnerBase transform_ = tf_buffer_->lookupTransform(mapFrame, robotFrame, tf2::TimePointZero); // trigger the events related to on_success state - if (events[execute_id].on_success.interface != "") + if (events[id].on_success.interface != "") { - event_(EventType::SUCCEEDED, id, events[execute_id].on_success.interface, - events[execute_id].on_success.provider); - triggerFunction_(events[execute_id].on_success.interface, - update_on_success(events[execute_id].on_success.parameters)); + event_(EventType::SUCCEEDED, id, events[id].on_success.interface, events[id].on_success.provider); + triggerFunction_(events[id].on_success.interface, update_on_success(events[id].on_success.parameters)); } info_("Transformation received. Thread closing.", id); @@ -109,12 +104,10 @@ class RobotPoseRunner : public RunnerBase transform_ = tf_buffer_->lookupTransform(odomFrame, robotFrame, tf2::TimePointZero); // trigger the events related to on_success state - if (events[execute_id].on_success.interface != "") + if (events[id].on_success.interface != "") { - event_(EventType::SUCCEEDED, id, events[execute_id].on_success.interface, - events[execute_id].on_success.provider); - triggerFunction_(events[execute_id].on_success.interface, - update_on_success(events[execute_id].on_success.parameters)); + event_(EventType::SUCCEEDED, id, events[id].on_success.interface, events[id].on_success.provider); + triggerFunction_(events[id].on_success.interface, update_on_success(events[id].on_success.parameters)); } info_("Transformation received. Thread closing.", id); @@ -124,11 +117,10 @@ class RobotPoseRunner : public RunnerBase info_("Could not transform from odom to robot: " + std::string(ex.what()), id); // trigger the events related to on_failure state - if (events[execute_id].on_failure.interface != "") + if (events[id].on_failure.interface != "") { - event_(EventType::FAILED, id, events[execute_id].on_failure.interface, events[execute_id].on_failure.provider); - triggerFunction_(events[execute_id].on_failure.interface, - update_on_failure(events[execute_id].on_failure.parameters)); + event_(EventType::FAILED, id, events[id].on_failure.interface, events[id].on_failure.provider); + triggerFunction_(events[id].on_failure.interface, update_on_failure(events[id].on_failure.parameters)); } info_("Transformation not received. Thread closing.", id); @@ -154,12 +146,11 @@ class RobotPoseRunner : public RunnerBase throw runner_exception("cannot stop runner subscriber that was not started"); // Trigger on_stopped event if defined - if (events[execute_id].on_stopped.interface != "") + if (events[runner_id].on_stopped.interface != "") { - event_(EventType::STOPPED, -1, events[execute_id].on_stopped.interface, - events[execute_id].on_stopped.provider); - triggerFunction_(events[execute_id].on_stopped.interface, - update_on_stopped(events[execute_id].on_stopped.parameters)); + event_(EventType::STOPPED, -1, events[runner_id].on_stopped.interface, events[runner_id].on_stopped.provider); + triggerFunction_(events[runner_id].on_stopped.interface, + update_on_stopped(events[runner_id].on_stopped.parameters)); } } From 730f5f92368d8acf237ee9dfc131ce7dbc7f3495 Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Fri, 22 Aug 2025 13:15:49 +1000 Subject: [PATCH 126/133] moved capability interfaces,providers and semantic interfaces together --- .../capabilities2_launch/__init__.py | 0 .../capabilities2_launch.py | 146 --------- capabilities2_launch/launch/server.launch.py | 37 --- .../launch_server/__init__.py | 0 .../launch_server/launch_process.py | 140 --------- capabilities2_launch/launch_server/server.py | 65 ---- capabilities2_launch/package.xml | 23 -- .../resource/capabilities2_launch | 0 capabilities2_launch/setup.cfg | 4 - capabilities2_launch/setup.py | 29 -- capabilities2_launch/test/test_copyright.py | 25 -- capabilities2_launch/test/test_flake8.py | 25 -- capabilities2_launch/test/test_pep257.py | 23 -- .../capabilities2_launch_proxy/__init__.py | 0 .../capabilities_launch_proxy.py | 280 ------------------ capabilities2_launch_proxy/package.xml | 19 -- capabilities2_launch_proxy/readme.md | 46 --- .../resource/capabilities2_launch_proxy | 0 capabilities2_launch_proxy/setup.cfg | 4 - capabilities2_launch_proxy/setup.py | 26 -- .../test/call_use_launch_runner.py | 94 ------ capabilities2_runner_audio/.clang-format | 64 ---- capabilities2_runner_audio/CMakeLists.txt | 64 ---- .../listen_runner.hpp | 96 ------ .../speak_runner.hpp | 75 ----- capabilities2_runner_audio/package.xml | 38 --- capabilities2_runner_audio/plugins.xml | 12 - .../src/audio_runners.cpp | 8 - capabilities2_runner_bt/.clang-format | 64 ---- capabilities2_runner_bt/CMakeLists.txt | 62 ---- .../bt_factory_runner.hpp | 87 ------ .../bt_runner_base.hpp | 109 ------- capabilities2_runner_bt/package.xml | 36 --- capabilities2_runner_bt/plugins.xml | 6 - capabilities2_runner_bt/readme.md | 35 --- capabilities2_runner_bt/src/bt_runners.cpp | 5 - .../CMakeLists.txt | 8 + .../interfaces/CapabilityGetRunner.yaml | 34 +++ capabilities2_runner_capabilities/package.xml | 9 + .../providers/CapabilityGetRunner.yaml | 9 + capabilities2_runner_fabric/.clang-format | 64 ---- capabilities2_runner_fabric/CMakeLists.txt | 63 ---- .../set_plan_runner.hpp | 61 ---- capabilities2_runner_fabric/package.xml | 34 --- capabilities2_runner_fabric/plugins.xml | 7 - .../src/fabric_runners.cpp | 6 - capabilities2_runner_nav2/.clang-format | 64 ---- capabilities2_runner_nav2/CMakeLists.txt | 69 ----- .../occupancygrid_runner.hpp | 132 --------- .../robotpose_runner.hpp | 207 ------------- .../waypoint_runner.hpp | 97 ------ capabilities2_runner_nav2/package.xml | 34 --- capabilities2_runner_nav2/plugins.xml | 17 -- .../src/nav2_runners.cpp | 10 - capabilities2_runner_prompt/.clang-format | 64 ---- capabilities2_runner_prompt/CMakeLists.txt | 63 ---- .../prompt_capability_runner.hpp | 44 --- .../prompt_occupancy_runner.hpp | 47 --- .../prompt_plan_runner.hpp | 93 ------ .../prompt_pose_runner.hpp | 46 --- .../prompt_service_runner.hpp | 91 ------ .../prompt_text_runner.hpp | 45 --- capabilities2_runner_prompt/package.xml | 33 --- capabilities2_runner_prompt/plugins.xml | 27 -- .../src/prompt_runners.cpp | 14 - capabilities2_runner_system/CMakeLists.txt | 8 + .../interfaces/CompletionRunner.yaml | 0 .../interfaces/InputMultiplexAllRunner.yaml | 0 .../interfaces/InputMultiplexAnyRunner.yaml | 0 capabilities2_runner_system/package.xml | 28 ++ .../providers/CompletionRunner.yaml | 0 .../providers/InputMultiplexAllRunner.yaml | 0 .../providers/InputMultiplexAnyRunner.yaml | 0 system_capabilities/CMakeLists.txt | 16 - system_capabilities/LICENSE | 17 -- system_capabilities/package.xml | 48 --- 76 files changed, 96 insertions(+), 3260 deletions(-) delete mode 100644 capabilities2_launch/capabilities2_launch/__init__.py delete mode 100644 capabilities2_launch/capabilities2_launch/capabilities2_launch.py delete mode 100644 capabilities2_launch/launch/server.launch.py delete mode 100644 capabilities2_launch/launch_server/__init__.py delete mode 100644 capabilities2_launch/launch_server/launch_process.py delete mode 100644 capabilities2_launch/launch_server/server.py delete mode 100644 capabilities2_launch/package.xml delete mode 100644 capabilities2_launch/resource/capabilities2_launch delete mode 100644 capabilities2_launch/setup.cfg delete mode 100644 capabilities2_launch/setup.py delete mode 100644 capabilities2_launch/test/test_copyright.py delete mode 100644 capabilities2_launch/test/test_flake8.py delete mode 100644 capabilities2_launch/test/test_pep257.py delete mode 100644 capabilities2_launch_proxy/capabilities2_launch_proxy/__init__.py delete mode 100755 capabilities2_launch_proxy/capabilities2_launch_proxy/capabilities_launch_proxy.py delete mode 100644 capabilities2_launch_proxy/package.xml delete mode 100644 capabilities2_launch_proxy/readme.md delete mode 100644 capabilities2_launch_proxy/resource/capabilities2_launch_proxy delete mode 100644 capabilities2_launch_proxy/setup.cfg delete mode 100644 capabilities2_launch_proxy/setup.py delete mode 100644 capabilities2_launch_proxy/test/call_use_launch_runner.py delete mode 100644 capabilities2_runner_audio/.clang-format delete mode 100644 capabilities2_runner_audio/CMakeLists.txt delete mode 100644 capabilities2_runner_audio/include/capabilities2_runner_audio/listen_runner.hpp delete mode 100644 capabilities2_runner_audio/include/capabilities2_runner_audio/speak_runner.hpp delete mode 100644 capabilities2_runner_audio/package.xml delete mode 100644 capabilities2_runner_audio/plugins.xml delete mode 100644 capabilities2_runner_audio/src/audio_runners.cpp delete mode 100644 capabilities2_runner_bt/.clang-format delete mode 100644 capabilities2_runner_bt/CMakeLists.txt delete mode 100644 capabilities2_runner_bt/include/capabilities2_runner_bt/bt_factory_runner.hpp delete mode 100644 capabilities2_runner_bt/include/capabilities2_runner_bt/bt_runner_base.hpp delete mode 100644 capabilities2_runner_bt/package.xml delete mode 100644 capabilities2_runner_bt/plugins.xml delete mode 100644 capabilities2_runner_bt/readme.md delete mode 100644 capabilities2_runner_bt/src/bt_runners.cpp create mode 100644 capabilities2_runner_capabilities/interfaces/CapabilityGetRunner.yaml create mode 100644 capabilities2_runner_capabilities/providers/CapabilityGetRunner.yaml delete mode 100644 capabilities2_runner_fabric/.clang-format delete mode 100644 capabilities2_runner_fabric/CMakeLists.txt delete mode 100644 capabilities2_runner_fabric/include/capabilities2_runner_fabric/set_plan_runner.hpp delete mode 100644 capabilities2_runner_fabric/package.xml delete mode 100644 capabilities2_runner_fabric/plugins.xml delete mode 100644 capabilities2_runner_fabric/src/fabric_runners.cpp delete mode 100644 capabilities2_runner_nav2/.clang-format delete mode 100644 capabilities2_runner_nav2/CMakeLists.txt delete mode 100644 capabilities2_runner_nav2/include/capabilities2_runner_nav2/occupancygrid_runner.hpp delete mode 100644 capabilities2_runner_nav2/include/capabilities2_runner_nav2/robotpose_runner.hpp delete mode 100644 capabilities2_runner_nav2/include/capabilities2_runner_nav2/waypoint_runner.hpp delete mode 100644 capabilities2_runner_nav2/package.xml delete mode 100644 capabilities2_runner_nav2/plugins.xml delete mode 100644 capabilities2_runner_nav2/src/nav2_runners.cpp delete mode 100644 capabilities2_runner_prompt/.clang-format delete mode 100644 capabilities2_runner_prompt/CMakeLists.txt delete mode 100644 capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_capability_runner.hpp delete mode 100644 capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_occupancy_runner.hpp delete mode 100644 capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_runner.hpp delete mode 100644 capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_pose_runner.hpp delete mode 100644 capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_service_runner.hpp delete mode 100644 capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_text_runner.hpp delete mode 100644 capabilities2_runner_prompt/package.xml delete mode 100644 capabilities2_runner_prompt/plugins.xml delete mode 100644 capabilities2_runner_prompt/src/prompt_runners.cpp rename {system_capabilities => capabilities2_runner_system}/interfaces/CompletionRunner.yaml (100%) rename {system_capabilities => capabilities2_runner_system}/interfaces/InputMultiplexAllRunner.yaml (100%) rename {system_capabilities => capabilities2_runner_system}/interfaces/InputMultiplexAnyRunner.yaml (100%) rename {system_capabilities => capabilities2_runner_system}/providers/CompletionRunner.yaml (100%) rename {system_capabilities => capabilities2_runner_system}/providers/InputMultiplexAllRunner.yaml (100%) rename {system_capabilities => capabilities2_runner_system}/providers/InputMultiplexAnyRunner.yaml (100%) delete mode 100644 system_capabilities/CMakeLists.txt delete mode 100644 system_capabilities/LICENSE delete mode 100644 system_capabilities/package.xml diff --git a/capabilities2_launch/capabilities2_launch/__init__.py b/capabilities2_launch/capabilities2_launch/__init__.py deleted file mode 100644 index e69de29..0000000 diff --git a/capabilities2_launch/capabilities2_launch/capabilities2_launch.py b/capabilities2_launch/capabilities2_launch/capabilities2_launch.py deleted file mode 100644 index 854c232..0000000 --- a/capabilities2_launch/capabilities2_launch/capabilities2_launch.py +++ /dev/null @@ -1,146 +0,0 @@ -import os -import signal -import rclpy -from rclpy.node import Node -from capabilities2_msgs.srv import Launch -from multiprocessing.connection import Client - -class LaunchServer(Node): - def __init__(self): - super().__init__('capabilities2_launch_server') - self.address = ('localhost', 6000) - self.client = Client(self.address, authkey=b'capabilities2') - - # Service for starting a launch file - self.start_server = self.create_service( - Launch, - '/capabilities/launch/start', - self.start_request - ) - - # Service for stopping a launch file - self.stop_server = self.create_service( - Launch, - '/capabilities/launch/stop', - self.stop_request - ) - - # Service for stopping a launch file - self.stop_server = self.create_service( - Launch, - '/capabilities/launch/status', - self.status_request - ) - - self.get_logger().info('Capabilities2 LaunchServer is ready.') - - def start_request(self, request, response): - package_name = request.package_name - launch_file_name = request.launch_file_name - - self.get_logger().info(f'[{package_name}/{launch_file_name}] received start request') - - message_outgoing = {} - message_outgoing["command"] = "start" - message_outgoing["package"] = package_name - message_outgoing["launch_file"] = launch_file_name - - self.get_logger().info(f'[{package_name}/{launch_file_name}] sending start request to Launch Server') - - self.client.send(message_outgoing) - - self.get_logger().info(f'[{package_name}/{launch_file_name}] waiting for response from Launch Server') - - message_incoming = self.client.recv() - response_content = message_incoming["status"] - - self.get_logger().info(f'[{package_name}/{launch_file_name}] received response : {response_content}') - - response.status = response_content - - return response - - def stop_request(self, request, response): - package_name = request.package_name - launch_file_name = request.launch_file_name - - self.get_logger().info(f'[{package_name}/{launch_file_name}] received stop request') - - message_outgoing = {} - message_outgoing["command"] = "stop" - message_outgoing["package"] = package_name - message_outgoing["launch_file"] = launch_file_name - - self.get_logger().info(f'[{package_name}/{launch_file_name}] sending stop request to Launch Server') - - self.client.send(message_outgoing) - - self.get_logger().info(f'[{package_name}/{launch_file_name}] waiting for response from Launch Server') - - message_incoming = self.client.recv() - response_content = message_incoming["status"] - - self.get_logger().info(f'[{package_name}/{launch_file_name}] received response : {response_content}') - - response.status = response_content - - return response - - - def status_request(self, request, response): - package_name = request.package_name - launch_file_name = request.launch_file_name - - self.get_logger().info(f'[{package_name}/{launch_file_name}] received status request') - - message_outgoing = {} - message_outgoing["command"] = "status" - message_outgoing["package"] = package_name - message_outgoing["launch_file"] = launch_file_name - - self.get_logger().info(f'[{package_name}/{launch_file_name}] sending status request to Launch Server') - - self.client.send(message_outgoing) - - self.get_logger().info(f'[{package_name}/{launch_file_name}] waiting for response from Launch Server') - - message_incoming = self.client.recv() - response_content = message_incoming["status"] - - self.get_logger().info(f'[{package_name}/{launch_file_name}] received response : {response_content}') - - response.status = response_content - - return response - - def shutdown(self): - - self.get_logger().info(f'Shutting down LaunchFileServer...') - - message_outgoing = {} - message_outgoing["command"] = "exit" - message_outgoing["package"] = "" - message_outgoing["launch_file"] = "" - - self.get_logger().info(f'[sending shutdown request to Launch Server') - - self.client.send(message_outgoing) - - -def main(args=None): - rclpy.init(args=args) - - # Create the LaunchServer node - launch_server = LaunchServer() - - # Keep the node running - try: - rclpy.spin(launch_server) - except KeyboardInterrupt: - launch_server.shutdown() - finally: - rclpy.shutdown() - - -if __name__ == '__main__': - main() \ No newline at end of file diff --git a/capabilities2_launch/launch/server.launch.py b/capabilities2_launch/launch/server.launch.py deleted file mode 100644 index 8ce3c51..0000000 --- a/capabilities2_launch/launch/server.launch.py +++ /dev/null @@ -1,37 +0,0 @@ -''' -capabilities2_server launch file -''' - -import os -from launch import LaunchDescription -from launch.actions import ExecuteProcess -from launch_ros.actions import Node -from launch.substitutions import FindExecutable -from ament_index_python.packages import get_package_share_directory - - -def generate_launch_description(): - """Generate launch description for capabilities2 server - - Returns: - LaunchDescription: The launch description for capabilities2 server - """ - - filePath = os.path.join(get_package_share_directory('capabilities2_launch'), 'launch_server', 'server.py') - - # create launch proxy node - launch_interface = Node( - package='capabilities2_launch', - executable='capabilities2_launch', - name='capabilities2_launch', - output='screen', - arguments=['--ros-args', '--log-level', 'info'] - ) - - server_process = ExecuteProcess(cmd=[[FindExecutable(name='python3'), ' ', filePath]], shell=True, output='screen') - - # return - return LaunchDescription([ - launch_interface, - server_process - ]) diff --git a/capabilities2_launch/launch_server/__init__.py b/capabilities2_launch/launch_server/__init__.py deleted file mode 100644 index e69de29..0000000 diff --git a/capabilities2_launch/launch_server/launch_process.py b/capabilities2_launch/launch_server/launch_process.py deleted file mode 100644 index 04af439..0000000 --- a/capabilities2_launch/launch_server/launch_process.py +++ /dev/null @@ -1,140 +0,0 @@ -from multiprocessing import Process -import launch -from ros2launch.api.api import parse_launch_arguments -from ros2launch.api import get_share_file_path_from_package -from ament_index_python.packages import PackageNotFoundError -from ros2launch.api import MultipleLaunchFilesError - -class LaunchProcess(Process): - - def __init__( - self, - package_name, - launch_file_name, - launch_file_arguments=[], - option_extensions={}, - noninteractive=True, - debug=False, - args=None - ): - """ - LaunchProcess objects represent ros2 launch files that need to be started is run in a separate process - - :param: package_name name of the package where launch file resides - :param: launch_file_name name of the launch file - :param: launch_file_arguments launch file arguments as a list - :param: option_extensions option extensions as a dictionary - :param: noninteractive flag for non-interactiveness - :param: debug flag for enabling debug info - :param: args additional arguments - """ - super().__init__() - self.package_name = package_name - self.launch_file_name = launch_file_name - self.launch_file_arguments = launch_file_arguments - self.noninteractive = noninteractive - self.debug = debug - self.option_extensions=option_extensions - self.args = args - self.path = None - - try: - self.path = get_share_file_path_from_package( - package_name=self.package_name, - file_name=self.launch_file_name) - - except PackageNotFoundError as exc: - raise RuntimeError(f"Package '{self.package_name}' not found: {exc}") - except (FileNotFoundError, MultipleLaunchFilesError) as exc: - raise RuntimeError(str(exc)) - - def run(self): - print(f"Launch file {self.launch_file_name} from package {self.package_name} started with pid [{self.pid}") - - launch_service = launch.LaunchService( - argv=self.launch_file_arguments, - noninteractive=self.noninteractive, - debug=self.debug) - - parsed_launch_arguments = parse_launch_arguments(self.launch_file_arguments) - - launch_description = launch.LaunchDescription([ - launch.actions.IncludeLaunchDescription( - launch.launch_description_sources.AnyLaunchDescriptionSource( - self.path - ), - launch_arguments=parsed_launch_arguments, - ), - ]) - - for name in sorted(self.option_extensions.keys()): - result = self.option_extensions[name].prelaunch(launch_description, self.args) - launch_description = result[0] - - launch_service.include_launch_description(launch_description) - - launch_service.run() - - def stop(self): - print(f"Stopping Launch file {self.launch_file_name} from package {self.package_name}") - self.kill() - -class LaunchManager: - def __init__(self): - self.processes = {} - - def start(self, package_name, launch_file_name): - name = package_name + "/" + launch_file_name - - if name not in self.processes: - try: - self.processes[name] = LaunchProcess(package_name=package_name, launch_file_name=launch_file_name) - self.processes[name].start() - except: - return {"status": "error occured"} - else: - return {"status": "successfuly started"} - else: - return {"status": "already started. ignoring"} - - def status(self, package_name, launch_file_name): - name = package_name + "/" + launch_file_name - - if name in self.processes: - if self.processes[name].is_alive(): - return {"status": "running"} - else: - return {"status": "failed"} - else: - return {"status": "stopped"} - - - def stop(self, package_name, launch_file_name): - name = package_name + "/" + launch_file_name - - if name in self.processes: - try: - self.processes[name].stop() - self.processes[name].join() - except: - return {"status": "error occured"} - else: - return {"status": "successfuly stopped"} - else: - return {"status": "already stopped. ignoring"} - - -if __name__ == "__main__": - launch_process = LaunchProcess( - package_name="nav_stack", - launch_file_name="system.launch.py" - ) - - launch_process.start() # Start the process - - import time - time.sleep(10) # Let the process run for 10 seconds - - launch_process.stop() # Signal the process to stop - launch_process.join() # Wait for the process to terminate - print("Launch process has exited.") \ No newline at end of file diff --git a/capabilities2_launch/launch_server/server.py b/capabilities2_launch/launch_server/server.py deleted file mode 100644 index beb0251..0000000 --- a/capabilities2_launch/launch_server/server.py +++ /dev/null @@ -1,65 +0,0 @@ -from launch_server.launch_process import LaunchManager -from multiprocessing.connection import Listener - -class LaunchServer: - def __init__(self): - self.address = ('localhost', 6000) - self.listener = Listener(self.address, authkey=b'capabilities2') - self.manager = LaunchManager() - - def initialize(self): - self.connection = self.listener.accept() - print("[Launch Server] connection Accepted from {self.listener.last_accepted}") - - def run(self): - while True: - # message would be a dictionary with the format of {"command": , "package": , "launch_file": } - message_incoming = self.connection.recv() - - command = message_incoming["command"] - - if ((command=="start") or (command=="stop") or (command=="status")): - package_name = message_incoming["package"] - launch_file_name = message_incoming["launch_file"] - else: - package_name = "" - launch_file_name = "" - - if (command=="start"): - print("[Launch Server] start request for {package_name}/{launch_file_name}") - status = self.manager.start(package_name=package_name, launch_file_name=launch_file_name) - result = status["status"] - - print("[Launch Server] start response of {package_name}/{launch_file_name}: {result}") - - elif (command=="stop"): - print("[Launch Server] stop request for {package_name}/{launch_file_name}") - status = self.manager.stop(package_name=package_name, launch_file_name=launch_file_name) - result = status["status"] - - print("[Launch Server] stop response of {package_name}/{launch_file_name}: {result}") - - elif (command=="status"): - print("[Launch Server] status request for {package_name}/{launch_file_name}") - status = self.manager.status(package_name=package_name, launch_file_name=launch_file_name) - result = status["status"] - - print("[Launch Server] status response of {package_name}/{launch_file_name}: {result}") - - elif (command=="exit"): - break - - else: - print("[Launch Server] does not support command of type : {command}") - break - - self.connection.send(status) - - self.listener.close - - -if __name__ == "__main__": - - server = LaunchServer() - server.initialize() - server.run() \ No newline at end of file diff --git a/capabilities2_launch/package.xml b/capabilities2_launch/package.xml deleted file mode 100644 index 8b0c6fa..0000000 --- a/capabilities2_launch/package.xml +++ /dev/null @@ -1,23 +0,0 @@ - - - - capabilities2_launch - 0.0.0 - TODO: Package description - kalana - TODO: License declaration - - rclcpp - capabilities2_msgs - - ros2launch - - ament_copyright - ament_flake8 - ament_pep257 - python3-pytest - - - ament_python - - diff --git a/capabilities2_launch/resource/capabilities2_launch b/capabilities2_launch/resource/capabilities2_launch deleted file mode 100644 index e69de29..0000000 diff --git a/capabilities2_launch/setup.cfg b/capabilities2_launch/setup.cfg deleted file mode 100644 index 9844c9c..0000000 --- a/capabilities2_launch/setup.cfg +++ /dev/null @@ -1,4 +0,0 @@ -[develop] -script_dir=$base/lib/capabilities2_launch -[install] -install_scripts=$base/lib/capabilities2_launch diff --git a/capabilities2_launch/setup.py b/capabilities2_launch/setup.py deleted file mode 100644 index 856147b..0000000 --- a/capabilities2_launch/setup.py +++ /dev/null @@ -1,29 +0,0 @@ -from setuptools import find_packages, setup -import os -from glob import glob - -package_name = 'capabilities2_launch' - -setup( - name=package_name, - version='0.1.0', - packages=find_packages(exclude=['test']), - data_files=[ - ('share/ament_index/resource_index/packages', ['resource/' + package_name]), - ('share/' + package_name, ['package.xml']), - (os.path.join('share', package_name, 'launch'), glob(os.path.join('launch', '*launch.[pxy][yma]*'))), - (os.path.join('share', package_name, 'launch_server'), glob(os.path.join('launch_server', '*.py'))), - ], - install_requires=['setuptools'], - zip_safe=True, - maintainer='kalana', - maintainer_email='kalanaratnayake95@gmail.com', - description='A ROS 2 package to manage starting and stopping ROS 2 launch files through services.', - license='TODO: License declaration', - tests_require=['pytest'], - entry_points={ - 'console_scripts': [ - 'capabilities2_launch = capabilities2_launch.capabilities2_launch:main', - ], - }, -) diff --git a/capabilities2_launch/test/test_copyright.py b/capabilities2_launch/test/test_copyright.py deleted file mode 100644 index 97a3919..0000000 --- a/capabilities2_launch/test/test_copyright.py +++ /dev/null @@ -1,25 +0,0 @@ -# Copyright 2015 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from ament_copyright.main import main -import pytest - - -# Remove the `skip` decorator once the source file(s) have a copyright header -@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') -@pytest.mark.copyright -@pytest.mark.linter -def test_copyright(): - rc = main(argv=['.', 'test']) - assert rc == 0, 'Found errors' diff --git a/capabilities2_launch/test/test_flake8.py b/capabilities2_launch/test/test_flake8.py deleted file mode 100644 index 27ee107..0000000 --- a/capabilities2_launch/test/test_flake8.py +++ /dev/null @@ -1,25 +0,0 @@ -# Copyright 2017 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from ament_flake8.main import main_with_errors -import pytest - - -@pytest.mark.flake8 -@pytest.mark.linter -def test_flake8(): - rc, errors = main_with_errors(argv=[]) - assert rc == 0, \ - 'Found %d code style errors / warnings:\n' % len(errors) + \ - '\n'.join(errors) diff --git a/capabilities2_launch/test/test_pep257.py b/capabilities2_launch/test/test_pep257.py deleted file mode 100644 index b234a38..0000000 --- a/capabilities2_launch/test/test_pep257.py +++ /dev/null @@ -1,23 +0,0 @@ -# Copyright 2015 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from ament_pep257.main import main -import pytest - - -@pytest.mark.linter -@pytest.mark.pep257 -def test_pep257(): - rc = main(argv=['.', 'test']) - assert rc == 0, 'Found code style errors / warnings' diff --git a/capabilities2_launch_proxy/capabilities2_launch_proxy/__init__.py b/capabilities2_launch_proxy/capabilities2_launch_proxy/__init__.py deleted file mode 100644 index e69de29..0000000 diff --git a/capabilities2_launch_proxy/capabilities2_launch_proxy/capabilities_launch_proxy.py b/capabilities2_launch_proxy/capabilities2_launch_proxy/capabilities_launch_proxy.py deleted file mode 100755 index b1773c0..0000000 --- a/capabilities2_launch_proxy/capabilities2_launch_proxy/capabilities_launch_proxy.py +++ /dev/null @@ -1,280 +0,0 @@ -''' -capabilities launch proxy server - -implements a launch server responding to messages to launch and shutdown launch files -use in conjunction with capabilities2_server to launch capabilities - -acts like ros2 launch command but from an action interface -''' - -import os -import threading -from typing import Set -from typing import Text -from typing import Optional -import rclpy -from rclpy.executors import MultiThreadedExecutor -from rclpy.executors import ExternalShutdownException -import rclpy.logging -from rclpy.node import Node -from rclpy.action import ActionServer -from rclpy.action import CancelResponse -from rclpy.action.server import ServerGoalHandle -from launch import LaunchService -from launch import LaunchContext -from launch.event import Event -from launch.events.process import ShutdownProcess -from launch.event_handler import EventHandler -from launch.event_handlers import OnProcessStart -from launch.actions import ExecuteProcess -from launch.actions import EmitEvent -from launch.actions import RegisterEventHandler -from launch.launch_description_sources import AnyLaunchDescriptionSource -# from launch.some_entities_type import SomeEntitiesType -from capabilities2_msgs.action import Launch - - -class CancelLaunchGoalEvent(Event): - """ - Launch goal cancel event - - used when a launch goal is canceled - the result should be that the launch description associated with a goal - is shutdown in the running LaunchService - other goals should not be affected - """ - - name = 'launch.event.CancelLaunchGoal' - - def __init__(self, *, goal_handle: ServerGoalHandle) -> None: - """ - Create a LaunchGoalCancelEvent - """ - self.goal_handle = goal_handle - - -class CancelEventHandler(EventHandler): - """ - Cancel event handler - - handle cancel launch goal events - cancels a launch goal if it matches the goal handle passed in - """ - - def __init__(self, goal_handle: ServerGoalHandle, pids: Set[int]): - super().__init__(matcher=lambda event: isinstance(event, CancelLaunchGoalEvent)) - self.goal_handle = goal_handle - self.pids = pids - - def handle(self, event: Event, context: LaunchContext): - """ - handle event - """ - - super().handle(event, context) - - # check if event is a cancel launch goal event - if not isinstance(event, CancelLaunchGoalEvent): - return None - - # if the goals match then cancel - if event.goal_handle == self.goal_handle: - return EmitEvent(event=ShutdownProcess( - process_matcher=lambda action: action.process_details['pid'] in self.pids - )) - - return None - - -class CapabilitiesLaunchProxy(Node): - """ - Capabilities Launch proxy server - """ - - def __init__(self, node_name='capabilities_launch_proxy'): - # super class init - super().__init__(node_name) - - # active files set - self.active_launch_files: Set[Text] = set() - self.launch_set_lock = threading.Lock() - - # create launch action server - self.launch_sub = ActionServer( - self, - Launch, - '~/launch', - handle_accepted_callback=self.handle_accepted_cb, - execute_callback=self.execute_cb, - cancel_callback=self.cancel_cb - ) - - # create launch service - self.launch_service = LaunchService() - - # log start - self.get_logger().info('capabilities launch proxy started') - - # service interface - def get_active_launch_files_cb(self): - """ - get active launch files - """ - return self.active_launch_files - - # action interface - def handle_accepted_cb(self, goal_handle: ServerGoalHandle) -> None: - """ - handle accepted callback - """ - # start execution and detach - threading.Thread(target=goal_handle.execute).start() - - # execute callback is main launch feature - def execute_cb(self, goal_handle: ServerGoalHandle) -> Launch.Result: - """ - launch execute callback - """ - - # check if file exists - if not os.path.isfile(goal_handle.request.launch_file_path): - # file does not exist - self.get_logger().error("file does not exist: '{}'".format( - goal_handle.request.launch_file_path)) - - # abort goal - goal_handle.abort() - return Launch.Result() - - with self.launch_set_lock: - # launch context already exists guard - if goal_handle.request.launch_file_path in self.active_launch_files: - self.get_logger().error("launch already exists for source '{}'".format( - goal_handle.request.launch_file_path)) - - goal_handle.abort() - return Launch.Result() - - # add context to active files - self.active_launch_files.add(goal_handle.request.launch_file_path) - - # get launch description from file - description = AnyLaunchDescriptionSource( - goal_handle.request.launch_file_path).get_launch_description(self.launch_service.context) - - # set up process id get event handler - description_process_ids: Set[int] = set() - - # go through all actions and collect all processes (mostly nodes) in the description - for e in description.entities: - # check the type of entity is an execute process action - if isinstance(e, ExecuteProcess): - # register event handler for process start event - # on process start store the pid - description.add_action( - RegisterEventHandler(OnProcessStart( - target_action=e, - on_start=lambda event, context: description_process_ids.add( - event.pid) - )) - ) - - # add shutdown event to launch description - # add shutdown event handler to launch description - # this uses the cancel goal event handler and cancel goal event - description.add_action(RegisterEventHandler(CancelEventHandler( - goal_handle, - description_process_ids - ))) - - # add to launch service - self.launch_service.include_launch_description(description) - - # bind request to this thread - self.get_logger().info('launching: {}'.format( - goal_handle.request.launch_file_path - )) - - # sleep rate for holding this action open - rate = self.create_rate(1.0) - - # loop while not canceled - while rclpy.ok(): - # if cancelled - if goal_handle.is_cancel_requested: - self.get_logger().warn("launch canceled: '{}'".format( - goal_handle.request.launch_file_path - )) - - # shutdown context - self.launch_service.emit_event( - CancelLaunchGoalEvent(goal_handle=goal_handle)) - - # delete context from active files - with self.launch_set_lock: - self.active_launch_files.remove( - goal_handle.request.launch_file_path) - - goal_handle.canceled() - return Launch.Result() - - # send event feedback - feedback = Launch.Feedback() - feedback.event.type = 'launched' - goal_handle.publish_feedback(feedback) - - # spin indefinitely (sleep loop this thread) - rate.sleep() - - goal_handle.succeed() - return Launch.Result() - - def cancel_cb(self, goal_handle: ServerGoalHandle) -> CancelResponse: - """ - accept all cancellations - """ - return CancelResponse.ACCEPT - - -# main function -def main(): - """ - main function - """ - - try: - # init node - rclpy.init() - - # instantiate the capabilities launch server - capabilities_launch_proxy = CapabilitiesLaunchProxy() - - # spin node in new thread - executor = MultiThreadedExecutor() - executor.add_node(capabilities_launch_proxy) - - executor_thread = threading.Thread(target=executor.spin) - executor_thread.start() - - # run the launch service - capabilities_launch_proxy.get_logger().info('running LaunchService') - capabilities_launch_proxy.launch_service.run( - shutdown_when_idle=False) - - # cancel the launch services - capabilities_launch_proxy.launch_service.shutdown() - - executor.shutdown() - executor_thread.join() - capabilities_launch_proxy.destroy_node() - - # rclpy.shutdown() - - except ExternalShutdownException: - pass - - -# main -if __name__ == '__main__': - # run the proxy - main() diff --git a/capabilities2_launch_proxy/package.xml b/capabilities2_launch_proxy/package.xml deleted file mode 100644 index ab70338..0000000 --- a/capabilities2_launch_proxy/package.xml +++ /dev/null @@ -1,19 +0,0 @@ - - - - capabilities2_launch_proxy - 0.0.1 - ros2 launch proxy to support launching capabilities similar to ros1 capabilities - mik-p - - MIT - - rclpy - launch - std_msgs - capabilities2_msgs - - - ament_python - - diff --git a/capabilities2_launch_proxy/readme.md b/capabilities2_launch_proxy/readme.md deleted file mode 100644 index e749a41..0000000 --- a/capabilities2_launch_proxy/readme.md +++ /dev/null @@ -1,46 +0,0 @@ -# capabilities2_launch_proxy - -Provides an Action API proxy to the ros2 launch framework. Implements features that are only available in python since the `launch` API is not available in C++. - -Contains the essential features to implement the ros1 [`capabilities`](https://wiki.ros.org/capabilities) package launch functionality in ros2. - -Use in conjunction with `capabilities2_server` package, to provide a full capabilities server. - -## how it works - -Implements three main functions: - -| Function | Description | -| --- | --- | -| `launch` | Run a launch file requested a runtime via a network request | -| `shutdown` | shutdown a launch file that has been started | -| `events` | send capability events during operation | - -The lifecylce of these functions are typically handled by the capabilities server but could be used for other things. - -## Use without capabilities2 server - -The proxy provides an action called `~/launch`. The `goal` is a file path to launch from. The feedback provides events about the launch status. - -### Add to a launch file - -```python -from launch import LaunchDescription -from launch_ros.actions import Node - -def generate_launch_description(): - return LaunchDescription([ - # create launch proxy node - Node( - package='capabilities2_launch_proxy', - executable='capabilities_launch_proxy', - name='capabilities_launch_proxy' - ) - ]) -``` - -### Run standalone - -```bash -ros2 run capabilities2_launch_proxy capabilities_launch_proxy -``` diff --git a/capabilities2_launch_proxy/resource/capabilities2_launch_proxy b/capabilities2_launch_proxy/resource/capabilities2_launch_proxy deleted file mode 100644 index e69de29..0000000 diff --git a/capabilities2_launch_proxy/setup.cfg b/capabilities2_launch_proxy/setup.cfg deleted file mode 100644 index d3f4af5..0000000 --- a/capabilities2_launch_proxy/setup.cfg +++ /dev/null @@ -1,4 +0,0 @@ -[develop] -script_dir=$base/lib/capabilities2_launch_proxy -[install] -install_scripts=$base/lib/capabilities2_launch_proxy diff --git a/capabilities2_launch_proxy/setup.py b/capabilities2_launch_proxy/setup.py deleted file mode 100644 index 1707673..0000000 --- a/capabilities2_launch_proxy/setup.py +++ /dev/null @@ -1,26 +0,0 @@ -from setuptools import find_packages, setup - -package_name = 'capabilities2_launch_proxy' - -setup( - name=package_name, - version='0.0.1', - packages=find_packages(exclude=['test']), - data_files=[ - ('share/ament_index/resource_index/packages', - ['resource/' + package_name]), - ('share/' + package_name, ['package.xml']), - ], - install_requires=['setuptools'], - zip_safe=True, - maintainer='mik-p', - maintainer_email='mppritchard3@hotmail.com', - description='ros2 launch proxy to support launching capabilities similar to ros1 capabilities', - license='MIT', - tests_require=['pytest'], - entry_points={ - 'console_scripts': [ - 'capabilities_launch_proxy = capabilities2_launch_proxy.capabilities_launch_proxy:main', - ], - }, -) diff --git a/capabilities2_launch_proxy/test/call_use_launch_runner.py b/capabilities2_launch_proxy/test/call_use_launch_runner.py deleted file mode 100644 index f8d3f9c..0000000 --- a/capabilities2_launch_proxy/test/call_use_launch_runner.py +++ /dev/null @@ -1,94 +0,0 @@ -''' test call cap services ''' - -from bondpy import bondpy -import rclpy -from capabilities2_msgs.srv import EstablishBond -from capabilities2_msgs.srv import GetRunningCapabilities -from capabilities2_msgs.srv import UseCapability - - -def test_get_running_capabilities(n): - # get running capabilities - client = n.create_client( - GetRunningCapabilities, - '/capabilities/get_running_capabilities' - ) - - # wait for service - client.wait_for_service() - - # send request - request = GetRunningCapabilities.Request() - future = client.call_async(request) - rclpy.spin_until_future_complete(n, future) - - # print result - for s in future.result().running_capabilities: - print(s) - - -# main -if __name__ == '__main__': - - rclpy.init() - - node = rclpy.create_node('test_call_cap_srvs') - - # do tests - # test_get_interfaces_srv(node) - # test_get_semantic_interfaces_srv(node) - # test_get_providers_srv(node) - # test_get_capability_specs_srv(node) - - print("test use capability") - # get a bond - bond_cli = node.create_client( - EstablishBond, - '/capabilities/establish_bond' - ) - - # wait for service - bond_cli.wait_for_service() - - # send request - request = EstablishBond.Request() - future = bond_cli.call_async(request) - rclpy.spin_until_future_complete(node, future) - - # print result - print(future.result()) - # keep bond id - bond_id = future.result().bond_id - - # create a use capability request - use_client = node.create_client( - UseCapability, - '/capabilities/use_capability' - ) - - # wait for service - use_client.wait_for_service() - - # send request - request = UseCapability.Request() - request.capability = 'std_capabilities/empty' - request.preferred_provider = 'std_capabilities/empty' - request.bond_id = future.result().bond_id - future = use_client.call_async(request) - rclpy.spin_until_future_complete(node, future) - - # print result - print(future.result()) - - # get running capabilities - test_get_running_capabilities(node) - - # keep bond alive - bond = bondpy.Bond(node, "/capabilities/bonds", bond_id) - bond.start() - rclpy.spin(node) - - node.destroy_node() - rclpy.shutdown() - - exit(0) diff --git a/capabilities2_runner_audio/.clang-format b/capabilities2_runner_audio/.clang-format deleted file mode 100644 index d36804f..0000000 --- a/capabilities2_runner_audio/.clang-format +++ /dev/null @@ -1,64 +0,0 @@ - -BasedOnStyle: Google -AccessModifierOffset: -2 -ConstructorInitializerIndentWidth: 2 -AlignEscapedNewlinesLeft: false -AlignTrailingComments: true -AllowAllParametersOfDeclarationOnNextLine: false -AllowShortIfStatementsOnASingleLine: false -AllowShortLoopsOnASingleLine: false -AllowShortFunctionsOnASingleLine: None -AlwaysBreakTemplateDeclarations: true -AlwaysBreakBeforeMultilineStrings: false -BreakBeforeBinaryOperators: false -BreakBeforeTernaryOperators: false -BreakConstructorInitializersBeforeComma: true -BinPackParameters: true -ColumnLimit: 120 -ConstructorInitializerAllOnOneLineOrOnePerLine: true -DerivePointerBinding: false -PointerBindsToType: true -ExperimentalAutoDetectBinPacking: false -IndentCaseLabels: true -MaxEmptyLinesToKeep: 1 -NamespaceIndentation: None -ObjCSpaceBeforeProtocolList: true -PenaltyBreakBeforeFirstCallParameter: 19 -PenaltyBreakComment: 60 -PenaltyBreakString: 1 -PenaltyBreakFirstLessLess: 1000 -PenaltyExcessCharacter: 1000 -PenaltyReturnTypeOnItsOwnLine: 90 -SpacesBeforeTrailingComments: 2 -Cpp11BracedListStyle: false -Standard: Auto -IndentWidth: 2 -TabWidth: 2 -UseTab: Never -IndentFunctionDeclarationAfterType: false -SpacesInParentheses: false -SpacesInAngles: false -SpaceInEmptyParentheses: false -SpacesInCStyleCastParentheses: false -SpaceAfterControlStatementKeyword: true -SpaceBeforeAssignmentOperators: true -ContinuationIndentWidth: 4 -SortIncludes: false -SpaceAfterCStyleCast: false - -# Configure each individual brace in BraceWrapping -BreakBeforeBraces: Custom - -# Control of individual brace wrapping cases -BraceWrapping: { - AfterClass: 'true' - AfterControlStatement: 'true' - AfterEnum : 'true' - AfterFunction : 'true' - AfterNamespace : 'true' - AfterStruct : 'true' - AfterUnion : 'true' - BeforeCatch : 'true' - BeforeElse : 'true' - IndentBraces : 'false' -} diff --git a/capabilities2_runner_audio/CMakeLists.txt b/capabilities2_runner_audio/CMakeLists.txt deleted file mode 100644 index c03dd00..0000000 --- a/capabilities2_runner_audio/CMakeLists.txt +++ /dev/null @@ -1,64 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(capabilities2_runner_audio) - -# Default to C++17 -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) -endif() - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -# find dependencies -find_package(ament_cmake REQUIRED) -find_package(rclcpp REQUIRED) -find_package(rclcpp_action REQUIRED) -find_package(pluginlib REQUIRED) -find_package(hri_audio_msgs REQUIRED) -find_package(capabilities2_msgs REQUIRED) -find_package(capabilities2_runner REQUIRED) -find_package(capabilities2_utils REQUIRED) -find_package(event_logger REQUIRED) -find_package(event_logger_msgs REQUIRED) -find_package(tinyxml2_vendor REQUIRED) -find_package(TinyXML2 REQUIRED) # provided by tinyxml2 upstream, or tinyxml2_vendor - -include_directories( - include -) - -add_library(${PROJECT_NAME} SHARED - src/audio_runners.cpp -) - -ament_target_dependencies(${PROJECT_NAME} - rclcpp - rclcpp_action - pluginlib - hri_audio_msgs - capabilities2_msgs - capabilities2_runner - capabilities2_utils - event_logger - event_logger_msgs - TinyXML2 -) - -pluginlib_export_plugin_description_file(capabilities2_runner plugins.xml) - -install(TARGETS ${PROJECT_NAME} - EXPORT ${PROJECT_NAME} - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION bin -) - -install(DIRECTORY include/ - DESTINATION include -) - -ament_export_include_directories(include) -ament_export_libraries(${PROJECT_NAME}) - -ament_package() diff --git a/capabilities2_runner_audio/include/capabilities2_runner_audio/listen_runner.hpp b/capabilities2_runner_audio/include/capabilities2_runner_audio/listen_runner.hpp deleted file mode 100644 index ad86511..0000000 --- a/capabilities2_runner_audio/include/capabilities2_runner_audio/listen_runner.hpp +++ /dev/null @@ -1,96 +0,0 @@ -#pragma once - -#include - -#include -#include -#include - -#include - -#include - -namespace capabilities2_runner -{ - -/** - * @brief action runner base class - * - * Class to run speech_to_text action based capability - */ -class ListenerRunner : public ActionRunner -{ -public: - ListenerRunner() : 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 - */ - virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config) override - { - init_action(node, run_config, "speech_to_text"); - } - -protected: - /** - * @brief This generate goal function overrides the generate_goal() function from ActionRunner() - * - * @param parameters XMLElement that contains parameters in the format '' - * @return ActionT::Goal the generated goal - */ - virtual hri_audio_msgs::action::SpeechToText::Goal generate_goal(tinyxml2::XMLElement* parameters, int id) override - { - hri_audio_msgs::action::SpeechToText::Goal goal_msg; - - goal_msg.header.stamp = node_->get_clock()->now(); - - return goal_msg; - } - - /** - * @brief This generate feedback function overrides the generate_feedback() function from ActionRunner() - * - * @param msg feedback message from the action server - * @return std::string of feedback information - */ - virtual std::string - generate_feedback(const typename hri_audio_msgs::action::SpeechToText::Feedback::ConstSharedPtr msg) override - { - std::string feedback = ""; - return feedback; - } - - /** - * @brief Update on_success event parameters with new data if avaible. - * - * This function is used to inject new data into the XMLElement containing - * parameters related to the on_success trigger event - * - * A pattern needs to be implemented in the derived class - * - * @param parameters pointer to the XMLElement containing parameters - * @return pointer to the XMLElement containing updated parameters - */ - virtual std::string update_on_success(std::string& parameters) - { - tinyxml2::XMLElement* element = convert_to_xml(parameters); - - // Create the Pose element as a child of the existing parameters element - tinyxml2::XMLElement* textElement = element->GetDocument()->NewElement("Text"); - element->InsertEndChild(textElement); - textElement->SetText(result_->stt_text.c_str()); - - // Return the updated parameters element with Pose added as string - std::string result = convert_to_string(element); - - return result; - }; -}; - -} // namespace capabilities2_runner diff --git a/capabilities2_runner_audio/include/capabilities2_runner_audio/speak_runner.hpp b/capabilities2_runner_audio/include/capabilities2_runner_audio/speak_runner.hpp deleted file mode 100644 index bd2d623..0000000 --- a/capabilities2_runner_audio/include/capabilities2_runner_audio/speak_runner.hpp +++ /dev/null @@ -1,75 +0,0 @@ -#pragma once - -#include - -#include -#include -#include - -#include - -#include - -namespace capabilities2_runner -{ - -/** - * @brief action runner base class - * - * Class to run text_to_speech action based capability - */ -class SpeakerRunner : public ActionRunner -{ -public: - SpeakerRunner() : 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 - */ - virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config) override - { - init_action(node, run_config, "text_to_speech"); - } - -protected: - /** - * @brief This generate goal function overrides the generate_goal() function from ActionRunner() - * - * @param parameters XMLElement that contains parameters in the format '' - * @return ActionT::Goal the generated goal - */ - virtual hri_audio_msgs::action::TextToSpeech::Goal generate_goal(tinyxml2::XMLElement* parameters, int id) override - { - const char** text; - parameters->QueryStringAttribute("text", text); - std::string tts_text(*text); - - hri_audio_msgs::action::TextToSpeech::Goal goal_msg; - - goal_msg.header.stamp = node_->get_clock()->now(); - goal_msg.tts_text = tts_text; - - return goal_msg; - } - - /** - * @brief This generate feedback function overrides the generate_feedback() function from ActionRunner() - * - * @param msg feedback message from the action server - * @return std::string of feedback information - */ - virtual std::string - generate_feedback(const typename hri_audio_msgs::action::TextToSpeech::Feedback::ConstSharedPtr msg) override - { - std::string feedback = ""; - return feedback; - } -}; - -} // namespace capabilities2_runner \ No newline at end of file diff --git a/capabilities2_runner_audio/package.xml b/capabilities2_runner_audio/package.xml deleted file mode 100644 index edccb72..0000000 --- a/capabilities2_runner_audio/package.xml +++ /dev/null @@ -1,38 +0,0 @@ - - - - capabilities2_runner_audio - 0.0.0 - TODO: Package description - - Kalana Ratnayake - Kalana Ratnayake - - Kalana Ratnayake - - MIT - - ament_cmake - - rclcpp - rclcpp_action - pluginlib - std_msgs - hri_audio_msgs - capabilities2_msgs - capabilities2_runner - capabilities2_utils - event_logger - event_logger_msgs - tinyxml2_vendor - - - ros2launch - - ament_lint_auto - ament_lint_common - - - ament_cmake - - diff --git a/capabilities2_runner_audio/plugins.xml b/capabilities2_runner_audio/plugins.xml deleted file mode 100644 index a1355d0..0000000 --- a/capabilities2_runner_audio/plugins.xml +++ /dev/null @@ -1,12 +0,0 @@ - - - - A plugin that provide speech to text capability - - - - - A plugin that provide text to speech capability - - - \ No newline at end of file diff --git a/capabilities2_runner_audio/src/audio_runners.cpp b/capabilities2_runner_audio/src/audio_runners.cpp deleted file mode 100644 index 5fb11f4..0000000 --- a/capabilities2_runner_audio/src/audio_runners.cpp +++ /dev/null @@ -1,8 +0,0 @@ -#include -#include -#include -#include - -// register runner plugins -PLUGINLIB_EXPORT_CLASS(capabilities2_runner::ListenerRunner, capabilities2_runner::RunnerBase) -PLUGINLIB_EXPORT_CLASS(capabilities2_runner::SpeakerRunner, capabilities2_runner::RunnerBase) diff --git a/capabilities2_runner_bt/.clang-format b/capabilities2_runner_bt/.clang-format deleted file mode 100644 index d36804f..0000000 --- a/capabilities2_runner_bt/.clang-format +++ /dev/null @@ -1,64 +0,0 @@ - -BasedOnStyle: Google -AccessModifierOffset: -2 -ConstructorInitializerIndentWidth: 2 -AlignEscapedNewlinesLeft: false -AlignTrailingComments: true -AllowAllParametersOfDeclarationOnNextLine: false -AllowShortIfStatementsOnASingleLine: false -AllowShortLoopsOnASingleLine: false -AllowShortFunctionsOnASingleLine: None -AlwaysBreakTemplateDeclarations: true -AlwaysBreakBeforeMultilineStrings: false -BreakBeforeBinaryOperators: false -BreakBeforeTernaryOperators: false -BreakConstructorInitializersBeforeComma: true -BinPackParameters: true -ColumnLimit: 120 -ConstructorInitializerAllOnOneLineOrOnePerLine: true -DerivePointerBinding: false -PointerBindsToType: true -ExperimentalAutoDetectBinPacking: false -IndentCaseLabels: true -MaxEmptyLinesToKeep: 1 -NamespaceIndentation: None -ObjCSpaceBeforeProtocolList: true -PenaltyBreakBeforeFirstCallParameter: 19 -PenaltyBreakComment: 60 -PenaltyBreakString: 1 -PenaltyBreakFirstLessLess: 1000 -PenaltyExcessCharacter: 1000 -PenaltyReturnTypeOnItsOwnLine: 90 -SpacesBeforeTrailingComments: 2 -Cpp11BracedListStyle: false -Standard: Auto -IndentWidth: 2 -TabWidth: 2 -UseTab: Never -IndentFunctionDeclarationAfterType: false -SpacesInParentheses: false -SpacesInAngles: false -SpaceInEmptyParentheses: false -SpacesInCStyleCastParentheses: false -SpaceAfterControlStatementKeyword: true -SpaceBeforeAssignmentOperators: true -ContinuationIndentWidth: 4 -SortIncludes: false -SpaceAfterCStyleCast: false - -# Configure each individual brace in BraceWrapping -BreakBeforeBraces: Custom - -# Control of individual brace wrapping cases -BraceWrapping: { - AfterClass: 'true' - AfterControlStatement: 'true' - AfterEnum : 'true' - AfterFunction : 'true' - AfterNamespace : 'true' - AfterStruct : 'true' - AfterUnion : 'true' - BeforeCatch : 'true' - BeforeElse : 'true' - IndentBraces : 'false' -} diff --git a/capabilities2_runner_bt/CMakeLists.txt b/capabilities2_runner_bt/CMakeLists.txt deleted file mode 100644 index 25bace3..0000000 --- a/capabilities2_runner_bt/CMakeLists.txt +++ /dev/null @@ -1,62 +0,0 @@ -cmake_minimum_required(VERSION 3.10) -project(capabilities2_runner_bt) - -# Default to C++17 -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) -endif() - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -find_package(ament_cmake REQUIRED) -find_package(rclcpp REQUIRED) -find_package(rclcpp_action REQUIRED) -find_package(pluginlib REQUIRED) -find_package(capabilities2_msgs REQUIRED) -find_package(capabilities2_runner REQUIRED) -find_package(capabilities2_utils REQUIRED) -find_package(event_logger REQUIRED) -find_package(event_logger_msgs REQUIRED) -find_package(behaviortree_cpp REQUIRED) -find_package(tinyxml2_vendor REQUIRED) -find_package(TinyXML2 REQUIRED) # provided by tinyxml2 upstream, or tinyxml2_vendor - -include_directories( - include -) - -add_library(${PROJECT_NAME} SHARED - src/bt_runners.cpp -) - -ament_target_dependencies(${PROJECT_NAME} - rclcpp - rclcpp_action - pluginlib - capabilities2_msgs - capabilities2_runner - capabilities2_utils - event_logger - event_logger_msgs - behaviortree_cpp - TinyXML2 -) - -pluginlib_export_plugin_description_file(capabilities2_runner plugins.xml) - -install(TARGETS ${PROJECT_NAME} - EXPORT ${PROJECT_NAME} - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION bin -) - -install(DIRECTORY include/ - DESTINATION include -) - -ament_export_include_directories(include) -ament_export_libraries(${PROJECT_NAME}) -ament_package() diff --git a/capabilities2_runner_bt/include/capabilities2_runner_bt/bt_factory_runner.hpp b/capabilities2_runner_bt/include/capabilities2_runner_bt/bt_factory_runner.hpp deleted file mode 100644 index 846f1a8..0000000 --- a/capabilities2_runner_bt/include/capabilities2_runner_bt/bt_factory_runner.hpp +++ /dev/null @@ -1,87 +0,0 @@ -#pragma once - -#include -#include -#include - -namespace capabilities2_runner -{ - -/** - * @brief base runner class for a behaviortree factory - * - * The runner implements a behaviour defined by a behaviourtree - * and executes it in a behaviortree - * - */ -class BTRunnerBase : public RunnerBase, public BT::BehaviorTreeFactory -{ -public: - BTRunnerBase() : RunnerBase(), BT::BehaviorTreeFactory() - { - } - - virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config) - { - // init the runner base - init_base(node, run_config); - - // register (bt)actions from ROS plugins - try - { - this->registerFromROSPlugins(); - } - catch (const std::exception& e) // TODO: add more specific exception - { - throw runner_exception(e.what()); - } - } - - virtual void stop() - { - // reset the tree (this destroys the nodes) - tree_.reset(); - } - -protected: - /** - * @brief trigger the behaviour factory with the input data - * - * @param parameters - * @return std::optional)>> - */ - virtual void trigger(const std::string& parameters) override - { - tinyxml2::XMLElement* parameters_xml = convert_to_xml(parameters); - - // if parameters are not provided then cannot proceed - if (!parameters_xml) - throw runner_exception("cannot trigger action without parameters"); - - // create the tree (ptr) - tree_ = std::make_shared(this->createTreeFromText(parameters_xml->GetText())); - - // return the tick function - // // the caller can call this function to tick the tree - // std::function)> result_callback = - // [this](std::shared_ptr result) { - // // tick the tree - // BT::NodeStatus status = tree_->tickWhileRunning(); - - // // fill the result - // if (status == BT::NodeStatus::SUCCESS) - // result->SetText("OK"); - // else - // result->SetText("FAIL"); - - // return; - // }; - - // return result_callback; - } - - // the tree - std::shared_ptr tree_; -}; - -} // namespace capabilities2_runner diff --git a/capabilities2_runner_bt/include/capabilities2_runner_bt/bt_runner_base.hpp b/capabilities2_runner_bt/include/capabilities2_runner_bt/bt_runner_base.hpp deleted file mode 100644 index 8bf4dea..0000000 --- a/capabilities2_runner_bt/include/capabilities2_runner_bt/bt_runner_base.hpp +++ /dev/null @@ -1,109 +0,0 @@ -#pragma once - -/** - * @file bt_runner_base.hpp - * @brief This file is heavily inspired by the nav2_behavior_tree::BtActionNode implementation - * as it performs almost the same functionality - * - * The nav2_behavior_tree::BtActionNode is part of navigation2 - * which is licensed under the Apache 2.0 license. see the following - * license file for more details: - * - */ - -// Copyright (c) 2018 Intel Corporation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include -#include - -namespace capabilities2_runner -{ - -/** - * @brief base class for a behaviortree action - * - * This class is heavily inspired by the nav2_behavior_tree::BtActionNode - * - * The action implements a action runner and an action node to combine the two - * domain definitions of 'action' - * - * Through this class, an action can be run in a behaviortree which implements a ROS action - * - * @tparam ActionT - */ -template -class BTRunnerBase : public ActionRunner, public BT::ActionNodeBase -{ -public: - BTRunnerBase() : ActionRunner(), BT::ActionNodeBase() - { - } - - // init pattern for bt and runner - /** - * @brief initializer function for initializing the action runner in place of constructor due to plugin semantics - * - * TODO: this is getting pretty messy - * - * @param node - * @param run_config - * @param action_name - * @param on_started - * @param on_terminated - * @param on_stopped - * @param bt_tag_name - * @param conf - */ - virtual void init_bt(rclcpp::Node::SharedPtr node, const runner_opts& run_config, const std::string& action_name, - const std::string& bt_tag_name, const BT::NodeConfiguration& conf, - std::function on_started = nullptr, - std::function on_terminated = nullptr, - std::function on_stopped = nullptr) - { - ActionRunner::init_action(node, run_config, action_name, on_started, on_terminated, on_stopped); - - // FIXME: no init in action base - // BT::ActionNodeBase::init(bt_tag_name, conf); - } - - // bt methods that need to be overridden - BT::NodeStatus tick() override - { - // TODO: run the action client? - } - - void halt() override - { - // cancel the action client if it is running - // can be implemented with the stop virtual method - // from action runner - ActionRunner::stop(); - } - - // the action runner methods that need to be overridden - // these can be overridden when this class is inherited and the action runner - // action template is resolved - // - // see runner_base.hpp for more details - // - // virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config, - // std::function on_started = nullptr, - // std::function on_terminated = nullptr, - // std::function on_stopped = nullptr) override - // - // virtual void trigger(std::shared_ptr parameters = nullptr) override -}; - -} // namespace capabilities2_runner diff --git a/capabilities2_runner_bt/package.xml b/capabilities2_runner_bt/package.xml deleted file mode 100644 index 07668d9..0000000 --- a/capabilities2_runner_bt/package.xml +++ /dev/null @@ -1,36 +0,0 @@ - - - capabilities2_runner_bt - 0.0.1 - - Package for behaviour tree based capabilities2 runners - - - Michael Pritchard - mik-p - - Michael Pritchard - - MIT - - ament_cmake - - rclcpp - rclcpp_action - pluginlib - std_msgs - capabilities2_msgs - capabilities2_runner - capabilities2_utils - event_logger - event_logger_msgs - tinyxml2_vendor - behaviortree_cpp - - - uuid - - - ament_cmake - - diff --git a/capabilities2_runner_bt/plugins.xml b/capabilities2_runner_bt/plugins.xml deleted file mode 100644 index 0763f3e..0000000 --- a/capabilities2_runner_bt/plugins.xml +++ /dev/null @@ -1,6 +0,0 @@ - - - - - - diff --git a/capabilities2_runner_bt/readme.md b/capabilities2_runner_bt/readme.md deleted file mode 100644 index 16d8543..0000000 --- a/capabilities2_runner_bt/readme.md +++ /dev/null @@ -1,35 +0,0 @@ -# capabilities2_runner_bt - -Behavior tree runners for [capabilities2](../readme.md). This package provides a runner for the capabilities2 package that uses behavior trees to combine and execute composite runners in the capabilities framework. - -## Runners - -| Runner | Description | -| ------ | ----------- | -| `BTRunnerBase` | A base class for behavior tree runners. This class is used to implement the behavior tree runner for the capabilities2 package. It inherits from the [`ActionRunner`](../capabilities2_runner/readme.md) to join ROS action and behavior tree action concepts. | -| `BTFactoryRunner` | Load a behavior tree and run it as a capabilities2 runner (this just means that it runs in the capabilities2_server node). | - -### Provider Example - -The following example demonstrates how to use the `BTFactoryRunner` to load a behavior tree from a file and run it as a capabilities2 runner. - -```yaml -name: bt_ask_help -spec_version: "1.1" -spec_type: provider -implements: std_capabilities/ask_help -runner: capabilities2_runner_bt/BTFactoryRunner - -# the tree to load -definition: | - - - - - - - - - - -``` diff --git a/capabilities2_runner_bt/src/bt_runners.cpp b/capabilities2_runner_bt/src/bt_runners.cpp deleted file mode 100644 index 2f1582c..0000000 --- a/capabilities2_runner_bt/src/bt_runners.cpp +++ /dev/null @@ -1,5 +0,0 @@ -#include -#include - - -// PLUGINLIB_EXPORT_CLASS(capabilities2_runner::BTRunnerBase, capabilities2_runner::RunnerBase) diff --git a/capabilities2_runner_capabilities/CMakeLists.txt b/capabilities2_runner_capabilities/CMakeLists.txt index 6dc43ee..6aa29c9 100644 --- a/capabilities2_runner_capabilities/CMakeLists.txt +++ b/capabilities2_runner_capabilities/CMakeLists.txt @@ -55,6 +55,14 @@ install(DIRECTORY include/ DESTINATION include ) +install(DIRECTORY interfaces + DESTINATION share/${PROJECT_NAME} +) + +install(DIRECTORY providers + DESTINATION share/${PROJECT_NAME} +) + ament_export_include_directories(include) ament_export_libraries(${PROJECT_NAME}) diff --git a/capabilities2_runner_capabilities/interfaces/CapabilityGetRunner.yaml b/capabilities2_runner_capabilities/interfaces/CapabilityGetRunner.yaml new file mode 100644 index 0000000..79535fc --- /dev/null +++ b/capabilities2_runner_capabilities/interfaces/CapabilityGetRunner.yaml @@ -0,0 +1,34 @@ +%YAML 1.1 +--- +name: CapabilityGetRunner +spec_version: 1.1 +spec_type: interface +description: "This capability depends on the capabilities2 server functionalities and allows an decision making authority such as an + LLM extract capabilities of the robot. The capability can be trigged with an command such as + ''. This is included in + the default starting plan and the decision making authority such as an LLM does not need to include this in any generated + plans. The details for the Capaiblities are stored in a format as follows, + ' + + + name: Navigation + dependencies: + - MapServer + - PathPlanner + + + + + name: ObjectDetection + dependencies: + - Camera + - ImageProcessor + + + ' " + +interface: + services: + "/capabilities/get_capability_specs": + type: "capabilities2_msgs::srv::GetCapabilitySpecs" + description: "This capability focuses on extracting robot's capabilities and transfering them to decision making authority." \ No newline at end of file diff --git a/capabilities2_runner_capabilities/package.xml b/capabilities2_runner_capabilities/package.xml index c97cfee..53bff7d 100644 --- a/capabilities2_runner_capabilities/package.xml +++ b/capabilities2_runner_capabilities/package.xml @@ -30,5 +30,14 @@ ament_cmake + + + + interfaces/CapabilityGetRunner.yaml + + + + providers/CapabilityGetRunner.yaml + diff --git a/capabilities2_runner_capabilities/providers/CapabilityGetRunner.yaml b/capabilities2_runner_capabilities/providers/CapabilityGetRunner.yaml new file mode 100644 index 0000000..7202ee7 --- /dev/null +++ b/capabilities2_runner_capabilities/providers/CapabilityGetRunner.yaml @@ -0,0 +1,9 @@ +# the empty provider +%YAML 1.1 +--- +name: CapabilityGetRunner +spec_type: provider +spec_version: 1.1 +description: "The capability provider for extracting capabilities from server. This is used to identify the capabilities of the robot" +implements: std_capabilities/CapabilityGetRunner +runner: capabilities2_runner::CapabilityGetRunner \ No newline at end of file diff --git a/capabilities2_runner_fabric/.clang-format b/capabilities2_runner_fabric/.clang-format deleted file mode 100644 index d36804f..0000000 --- a/capabilities2_runner_fabric/.clang-format +++ /dev/null @@ -1,64 +0,0 @@ - -BasedOnStyle: Google -AccessModifierOffset: -2 -ConstructorInitializerIndentWidth: 2 -AlignEscapedNewlinesLeft: false -AlignTrailingComments: true -AllowAllParametersOfDeclarationOnNextLine: false -AllowShortIfStatementsOnASingleLine: false -AllowShortLoopsOnASingleLine: false -AllowShortFunctionsOnASingleLine: None -AlwaysBreakTemplateDeclarations: true -AlwaysBreakBeforeMultilineStrings: false -BreakBeforeBinaryOperators: false -BreakBeforeTernaryOperators: false -BreakConstructorInitializersBeforeComma: true -BinPackParameters: true -ColumnLimit: 120 -ConstructorInitializerAllOnOneLineOrOnePerLine: true -DerivePointerBinding: false -PointerBindsToType: true -ExperimentalAutoDetectBinPacking: false -IndentCaseLabels: true -MaxEmptyLinesToKeep: 1 -NamespaceIndentation: None -ObjCSpaceBeforeProtocolList: true -PenaltyBreakBeforeFirstCallParameter: 19 -PenaltyBreakComment: 60 -PenaltyBreakString: 1 -PenaltyBreakFirstLessLess: 1000 -PenaltyExcessCharacter: 1000 -PenaltyReturnTypeOnItsOwnLine: 90 -SpacesBeforeTrailingComments: 2 -Cpp11BracedListStyle: false -Standard: Auto -IndentWidth: 2 -TabWidth: 2 -UseTab: Never -IndentFunctionDeclarationAfterType: false -SpacesInParentheses: false -SpacesInAngles: false -SpaceInEmptyParentheses: false -SpacesInCStyleCastParentheses: false -SpaceAfterControlStatementKeyword: true -SpaceBeforeAssignmentOperators: true -ContinuationIndentWidth: 4 -SortIncludes: false -SpaceAfterCStyleCast: false - -# Configure each individual brace in BraceWrapping -BreakBeforeBraces: Custom - -# Control of individual brace wrapping cases -BraceWrapping: { - AfterClass: 'true' - AfterControlStatement: 'true' - AfterEnum : 'true' - AfterFunction : 'true' - AfterNamespace : 'true' - AfterStruct : 'true' - AfterUnion : 'true' - BeforeCatch : 'true' - BeforeElse : 'true' - IndentBraces : 'false' -} diff --git a/capabilities2_runner_fabric/CMakeLists.txt b/capabilities2_runner_fabric/CMakeLists.txt deleted file mode 100644 index b813db7..0000000 --- a/capabilities2_runner_fabric/CMakeLists.txt +++ /dev/null @@ -1,63 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(capabilities2_runner_fabric) - -# Default to C++17 -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) -endif() - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -find_package(ament_cmake REQUIRED) -find_package(rclcpp REQUIRED) -find_package(rclcpp_action REQUIRED) -find_package(pluginlib REQUIRED) -find_package(fabric_msgs REQUIRED) -find_package(capabilities2_msgs REQUIRED) -find_package(capabilities2_runner REQUIRED) -find_package(capabilities2_utils REQUIRED) -find_package(event_logger REQUIRED) -find_package(event_logger_msgs REQUIRED) -find_package(tinyxml2_vendor REQUIRED) -find_package(TinyXML2 REQUIRED) # provided by tinyxml2 upstream, or tinyxml2_vendor - -include_directories( - include -) - -add_library(${PROJECT_NAME} SHARED - src/fabric_runners.cpp -) - -ament_target_dependencies(${PROJECT_NAME} - rclcpp - rclcpp_action - pluginlib - fabric_msgs - capabilities2_msgs - capabilities2_runner - capabilities2_utils - event_logger - event_logger_msgs - TinyXML2 -) - -pluginlib_export_plugin_description_file(capabilities2_runner plugins.xml) - -install(TARGETS ${PROJECT_NAME} - EXPORT ${PROJECT_NAME} - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION bin -) - -install(DIRECTORY include/ - DESTINATION include -) - -ament_export_include_directories(include) -ament_export_libraries(${PROJECT_NAME}) - -ament_package() diff --git a/capabilities2_runner_fabric/include/capabilities2_runner_fabric/set_plan_runner.hpp b/capabilities2_runner_fabric/include/capabilities2_runner_fabric/set_plan_runner.hpp deleted file mode 100644 index 20eec05..0000000 --- a/capabilities2_runner_fabric/include/capabilities2_runner_fabric/set_plan_runner.hpp +++ /dev/null @@ -1,61 +0,0 @@ -#pragma once - -#include - -#include -#include -#include - -#include -#include - -namespace capabilities2_runner -{ - -/** - * @brief Executor runner class - * - * Class to run capabilities2 executor action based capability - * - */ -class FabricSetPlanRunner : public ServiceRunner -{ -public: - FabricSetPlanRunner() : ServiceRunner() - { - } - - /** - * @brief Starter function for starting the action runner - * - * @param node shared pointer to the capabilities node. Allows to use ros node related functionalities - * @param run_config runner configuration loaded from the yaml file - */ - virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config) override - { - init_service(node, run_config, "/fabric/set_plan"); - } - -protected: - /** - * @brief This generate goal function overrides the generate_goal() function from ActionRunner() - * @param parameters XMLElement that contains parameters in the format - * @return ActionT::Goal the generated goal - */ - virtual fabric_msgs::srv::SetFabricPlan::Request generate_request(tinyxml2::XMLElement* parameters, int id) override - { - tinyxml2::XMLElement* planElement = parameters->FirstChildElement("ReceievdPlan"); - - fabric_msgs::srv::SetFabricPlan::Request request; - - // Check if the element was found and has text content - if (planElement && planElement->GetText()) - { - request.plan = std::string(planElement->GetText()); - } - - return request; - } -}; - -} // namespace capabilities2_runner diff --git a/capabilities2_runner_fabric/package.xml b/capabilities2_runner_fabric/package.xml deleted file mode 100644 index d2b1197..0000000 --- a/capabilities2_runner_fabric/package.xml +++ /dev/null @@ -1,34 +0,0 @@ - - - - capabilities2_runner_fabric - 0.0.0 - TODO: Package description - - - Kalana Ratnayake - Kalana Ratnayake - - Kalana Ratnayake - - TODO: License declaration - - ament_cmake - - ament_lint_auto - ament_lint_common - - rclcpp - rclcpp_action - pluginlib - std_msgs - fabric_msgs - capabilities2_runner - event_logger - event_logger_msgs - tinyxml2_vendor - - - ament_cmake - - diff --git a/capabilities2_runner_fabric/plugins.xml b/capabilities2_runner_fabric/plugins.xml deleted file mode 100644 index 07e8a06..0000000 --- a/capabilities2_runner_fabric/plugins.xml +++ /dev/null @@ -1,7 +0,0 @@ - - - - A plugin that sets a new Fabric plan to the Fabric - - - diff --git a/capabilities2_runner_fabric/src/fabric_runners.cpp b/capabilities2_runner_fabric/src/fabric_runners.cpp deleted file mode 100644 index 703c1e1..0000000 --- a/capabilities2_runner_fabric/src/fabric_runners.cpp +++ /dev/null @@ -1,6 +0,0 @@ -#include -#include -#include - -// register runner plugins -PLUGINLIB_EXPORT_CLASS(capabilities2_runner::FabricSetPlanRunner, capabilities2_runner::RunnerBase) diff --git a/capabilities2_runner_nav2/.clang-format b/capabilities2_runner_nav2/.clang-format deleted file mode 100644 index d36804f..0000000 --- a/capabilities2_runner_nav2/.clang-format +++ /dev/null @@ -1,64 +0,0 @@ - -BasedOnStyle: Google -AccessModifierOffset: -2 -ConstructorInitializerIndentWidth: 2 -AlignEscapedNewlinesLeft: false -AlignTrailingComments: true -AllowAllParametersOfDeclarationOnNextLine: false -AllowShortIfStatementsOnASingleLine: false -AllowShortLoopsOnASingleLine: false -AllowShortFunctionsOnASingleLine: None -AlwaysBreakTemplateDeclarations: true -AlwaysBreakBeforeMultilineStrings: false -BreakBeforeBinaryOperators: false -BreakBeforeTernaryOperators: false -BreakConstructorInitializersBeforeComma: true -BinPackParameters: true -ColumnLimit: 120 -ConstructorInitializerAllOnOneLineOrOnePerLine: true -DerivePointerBinding: false -PointerBindsToType: true -ExperimentalAutoDetectBinPacking: false -IndentCaseLabels: true -MaxEmptyLinesToKeep: 1 -NamespaceIndentation: None -ObjCSpaceBeforeProtocolList: true -PenaltyBreakBeforeFirstCallParameter: 19 -PenaltyBreakComment: 60 -PenaltyBreakString: 1 -PenaltyBreakFirstLessLess: 1000 -PenaltyExcessCharacter: 1000 -PenaltyReturnTypeOnItsOwnLine: 90 -SpacesBeforeTrailingComments: 2 -Cpp11BracedListStyle: false -Standard: Auto -IndentWidth: 2 -TabWidth: 2 -UseTab: Never -IndentFunctionDeclarationAfterType: false -SpacesInParentheses: false -SpacesInAngles: false -SpaceInEmptyParentheses: false -SpacesInCStyleCastParentheses: false -SpaceAfterControlStatementKeyword: true -SpaceBeforeAssignmentOperators: true -ContinuationIndentWidth: 4 -SortIncludes: false -SpaceAfterCStyleCast: false - -# Configure each individual brace in BraceWrapping -BreakBeforeBraces: Custom - -# Control of individual brace wrapping cases -BraceWrapping: { - AfterClass: 'true' - AfterControlStatement: 'true' - AfterEnum : 'true' - AfterFunction : 'true' - AfterNamespace : 'true' - AfterStruct : 'true' - AfterUnion : 'true' - BeforeCatch : 'true' - BeforeElse : 'true' - IndentBraces : 'false' -} diff --git a/capabilities2_runner_nav2/CMakeLists.txt b/capabilities2_runner_nav2/CMakeLists.txt deleted file mode 100644 index 01483a7..0000000 --- a/capabilities2_runner_nav2/CMakeLists.txt +++ /dev/null @@ -1,69 +0,0 @@ -cmake_minimum_required(VERSION 3.10) -project(capabilities2_runner_nav2) - -# Default to C++17 -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) -endif() - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -find_package(ament_cmake REQUIRED) -find_package(rclcpp REQUIRED) -find_package(rclcpp_action REQUIRED) -find_package(pluginlib REQUIRED) -find_package(nav2_msgs REQUIRED) -find_package(tf2 REQUIRED) -find_package(tf2_ros REQUIRED) -find_package(geometry_msgs REQUIRED) -find_package(capabilities2_msgs REQUIRED) -find_package(capabilities2_runner REQUIRED) -find_package(capabilities2_utils REQUIRED) -find_package(event_logger REQUIRED) -find_package(event_logger_msgs REQUIRED) -find_package(tinyxml2_vendor REQUIRED) -find_package(TinyXML2 REQUIRED) # provided by tinyxml2 upstream, or tinyxml2_vendor - -include_directories( - include -) - -add_library(${PROJECT_NAME} SHARED - src/nav2_runners.cpp -) - -ament_target_dependencies(${PROJECT_NAME} - rclcpp - rclcpp_action - pluginlib - nav2_msgs - tf2 - tf2_ros - geometry_msgs - capabilities2_msgs - capabilities2_runner - capabilities2_utils - event_logger - event_logger_msgs - TinyXML2 -) - -pluginlib_export_plugin_description_file(capabilities2_runner plugins.xml) - -install(TARGETS ${PROJECT_NAME} - EXPORT ${PROJECT_NAME} - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION bin -) - -install(DIRECTORY include/ - DESTINATION include -) - -ament_export_include_directories(include) -ament_export_libraries(${PROJECT_NAME}) - -ament_package() diff --git a/capabilities2_runner_nav2/include/capabilities2_runner_nav2/occupancygrid_runner.hpp b/capabilities2_runner_nav2/include/capabilities2_runner_nav2/occupancygrid_runner.hpp deleted file mode 100644 index f979f28..0000000 --- a/capabilities2_runner_nav2/include/capabilities2_runner_nav2/occupancygrid_runner.hpp +++ /dev/null @@ -1,132 +0,0 @@ -#pragma once - -#include - -#include -#include - -#include - -namespace capabilities2_runner -{ - -/** - * @brief odometry runner class - * - * Capability Class to grab occupancy grid data - * - */ -class OccupancyGridRunner : public TopicRunner -{ -public: - OccupancyGridRunner() : TopicRunner() - { - } - - /** - * @brief Starter function for starting the subscription runner - * - * @param node shared pointer to the capabilities node. Allows to use ros node related functionalities - * @param run_config runner configuration loaded from the yaml file - */ - virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config) override - { - init_subscriber(node, run_config, "/map"); - } - -protected: - /** - * @brief Update on_success event parameters with new data from an OccupancyGrid message if avaible. - * - * This function is used to inject new data into the XMLElement containing - * parameters related to the on_success trigger event - * - -
- -
- - - - - - - - 0 100 -1 50 25 75 0 0 100 50 - - * - * @param parameters pointer to the XMLElement containing parameters - * @return pointer to the XMLElement containing updated parameters - */ - virtual std::string update_on_success(std::string& parameters) - { - tinyxml2::XMLElement* element = convert_to_xml(parameters); - - // Create the OccupancyGrid element as a child of the existing parameters element - tinyxml2::XMLElement* occupancyGridElement = element->GetDocument()->NewElement("OccupancyGrid"); - element->InsertEndChild(occupancyGridElement); - - // Header element with attributes - tinyxml2::XMLElement* headerElement = element->GetDocument()->NewElement("header"); - headerElement->SetAttribute("frame_id", latest_message_->header.frame_id.c_str()); - - tinyxml2::XMLElement* stampElement = element->GetDocument()->NewElement("stamp"); - stampElement->SetAttribute("secs", latest_message_->header.stamp.sec); - stampElement->SetAttribute("nsecs", latest_message_->header.stamp.nanosec); - headerElement->InsertEndChild(stampElement); - - occupancyGridElement->InsertEndChild(headerElement); - - // Info element with attributes - tinyxml2::XMLElement* infoElement = element->GetDocument()->NewElement("info"); - infoElement->SetAttribute("resolution", latest_message_->info.resolution); - infoElement->SetAttribute("width", latest_message_->info.width); - infoElement->SetAttribute("height", latest_message_->info.height); - - // Origin element with position and orientation attributes - tinyxml2::XMLElement* originElement = element->GetDocument()->NewElement("origin"); - - tinyxml2::XMLElement* positionElement = element->GetDocument()->NewElement("position"); - positionElement->SetAttribute("x", latest_message_->info.origin.position.x); - positionElement->SetAttribute("y", latest_message_->info.origin.position.y); - positionElement->SetAttribute("z", latest_message_->info.origin.position.z); - originElement->InsertEndChild(positionElement); - - tinyxml2::XMLElement* orientationElement = element->GetDocument()->NewElement("orientation"); - orientationElement->SetAttribute("x", latest_message_->info.origin.orientation.x); - orientationElement->SetAttribute("y", latest_message_->info.origin.orientation.y); - orientationElement->SetAttribute("z", latest_message_->info.origin.orientation.z); - orientationElement->SetAttribute("w", latest_message_->info.origin.orientation.w); - originElement->InsertEndChild(orientationElement); - - infoElement->InsertEndChild(originElement); - - // Map load time with attributes - tinyxml2::XMLElement* mapLoadTimeElement = element->GetDocument()->NewElement("map_load_time"); - mapLoadTimeElement->SetAttribute("secs", latest_message_->info.map_load_time.sec); - mapLoadTimeElement->SetAttribute("nsecs", latest_message_->info.map_load_time.nanosec); - infoElement->InsertEndChild(mapLoadTimeElement); - - occupancyGridElement->InsertEndChild(infoElement); - - // Data element for occupancy data (converted to a string) - tinyxml2::XMLElement* dataElement = element->GetDocument()->NewElement("data"); - - std::string data_str; - for (size_t i = 0; i < latest_message_->data.size(); i++) - { - data_str += std::to_string(latest_message_->data[i]) + " "; - } - dataElement->SetText(data_str.c_str()); - - occupancyGridElement->InsertEndChild(dataElement); - - // Return the updated parameters element with OccupancyGrid added - std::string result = convert_to_string(element); - - // output_("on_success trigger parameter", result); - - return result; - }; -}; -} // namespace capabilities2_runner \ No newline at end of file diff --git a/capabilities2_runner_nav2/include/capabilities2_runner_nav2/robotpose_runner.hpp b/capabilities2_runner_nav2/include/capabilities2_runner_nav2/robotpose_runner.hpp deleted file mode 100644 index 4f7d8bd..0000000 --- a/capabilities2_runner_nav2/include/capabilities2_runner_nav2/robotpose_runner.hpp +++ /dev/null @@ -1,207 +0,0 @@ -#pragma once - -#include - -#include -#include "geometry_msgs/msg/transform_stamped.hpp" - -#include - -#include "tf2/exceptions.h" -#include "tf2_ros/transform_listener.h" -#include "tf2_ros/buffer.h" - -namespace capabilities2_runner -{ - -/** - * @brief odometry runner class - * - * Capability Class to grab odometry data - * - */ -class RobotPoseRunner : public RunnerBase -{ -public: - RobotPoseRunner() : RunnerBase() - { - } - - /** - * @brief Starter function for starting the subscription runner - * - * @param node shared pointer to the capabilities node. Allows to use ros node related functionalities - * @param run_config runner configuration loaded from the yaml file - */ - virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config) override - { - // initialize the runner base by storing node pointer and run config - init_base(node, run_config); - - tf_buffer_ = std::make_unique(node_->get_clock()); - tf_listener_ = std::make_shared(*tf_buffer_); - } - - /** - * @brief Trigger process to be executed. - * - * This method utilizes paramters set via the trigger() function - * - * @param parameters pointer to tinyxml2::XMLElement that contains parameters - */ - virtual void execution(int id) override - { - const char* map; - const char* odom; - const char* robot; - - // if parameters are not provided then cannot proceed - if (!parameters_[id]) - throw runner_exception("cannot grab data without parameters"); - - // trigger the events related to on_started state - if (events[id].on_started.interface != "") - { - event_(EventType::STARTED, id, events[id].on_started.interface, events[id].on_started.provider); - triggerFunction_(events[id].on_started.interface, update_on_started(events[id].on_started.parameters)); - } - - info_("Waiting for Transformation.", id); - - parameters_[id]->QueryStringAttribute("map", &map); - parameters_[id]->QueryStringAttribute("odom", &odom); - parameters_[id]->QueryStringAttribute("robot", &robot); - - // Store frame names in variables that will be used to - // compute transformations - std::string mapFrame(map); - std::string odomFrame(odom); - std::string robotFrame(robot); - - // Try to use map -> robot first - try - { - transform_ = tf_buffer_->lookupTransform(mapFrame, robotFrame, tf2::TimePointZero); - - // trigger the events related to on_success state - if (events[id].on_success.interface != "") - { - event_(EventType::SUCCEEDED, id, events[id].on_success.interface, events[id].on_success.provider); - triggerFunction_(events[id].on_success.interface, update_on_success(events[id].on_success.parameters)); - } - - info_("Transformation received. Thread closing.", id); - return; - } - catch (tf2::TransformException& ex) - { - info_("Could not transform from map to robot: " + std::string(ex.what()), id); - } - - // Fall back to odom -> robot - try - { - transform_ = tf_buffer_->lookupTransform(odomFrame, robotFrame, tf2::TimePointZero); - - // trigger the events related to on_success state - if (events[id].on_success.interface != "") - { - event_(EventType::SUCCEEDED, id, events[id].on_success.interface, events[id].on_success.provider); - triggerFunction_(events[id].on_success.interface, update_on_success(events[id].on_success.parameters)); - } - - info_("Transformation received. Thread closing.", id); - } - catch (tf2::TransformException& ex) - { - info_("Could not transform from odom to robot: " + std::string(ex.what()), id); - - // trigger the events related to on_failure state - if (events[id].on_failure.interface != "") - { - event_(EventType::FAILED, id, events[id].on_failure.interface, events[id].on_failure.provider); - triggerFunction_(events[id].on_failure.interface, update_on_failure(events[id].on_failure.parameters)); - } - - info_("Transformation not received. Thread closing.", id); - } - } - - /** - * @brief stop function to cease functionality and shutdown - * - */ - virtual void stop() override - { - // if the node pointer is empty then throw an error - // this means that the runner was not started and is being used out of order - - if (!node_) - throw runner_exception("cannot stop runner that was not started"); - - // throw an error if the service client is null - // this can happen if the runner is not able to find the action resource - - if (!tf_listener_) - throw runner_exception("cannot stop runner subscriber that was not started"); - - // Trigger on_stopped event if defined - if (events[runner_id].on_stopped.interface != "") - { - event_(EventType::STOPPED, -1, events[runner_id].on_stopped.interface, events[runner_id].on_stopped.provider); - triggerFunction_(events[runner_id].on_stopped.interface, - update_on_stopped(events[runner_id].on_stopped.parameters)); - } - } - -protected: - /** - * @brief Update on_success event parameters with new data if avaible. - * - * This function is used to inject new data into the XMLElement containing - * parameters related to the on_success trigger event - * - - - - - * - * @param parameters pointer to the XMLElement containing parameters - * @return pointer to the XMLElement containing updated parameters - */ - virtual std::string update_on_success(std::string& parameters) - { - tinyxml2::XMLElement* element = convert_to_xml(parameters); - - // Create the Pose element as a child of the existing parameters element - tinyxml2::XMLElement* poseElement = element->GetDocument()->NewElement("Pose"); - element->InsertEndChild(poseElement); - - // Position element with attributes - tinyxml2::XMLElement* positionElement = element->GetDocument()->NewElement("position"); - positionElement->SetAttribute("x", transform_.transform.translation.x); - positionElement->SetAttribute("y", transform_.transform.translation.y); - positionElement->SetAttribute("z", transform_.transform.translation.z); - poseElement->InsertEndChild(positionElement); - - // Orientation element with attributes - tinyxml2::XMLElement* orientationElement = element->GetDocument()->NewElement("orientation"); - orientationElement->SetAttribute("x", transform_.transform.rotation.x); - orientationElement->SetAttribute("y", transform_.transform.rotation.y); - orientationElement->SetAttribute("z", transform_.transform.rotation.z); - orientationElement->SetAttribute("w", transform_.transform.rotation.w); - poseElement->InsertEndChild(orientationElement); - - // Return the updated parameters element with Pose added as string - std::string result = convert_to_string(element); - - // output_("on_success trigger parameter", result); - - return result; - }; - - std::shared_ptr tf_listener_{ nullptr }; - std::unique_ptr tf_buffer_; - geometry_msgs::msg::TransformStamped transform_; -}; -} // namespace capabilities2_runner \ No newline at end of file diff --git a/capabilities2_runner_nav2/include/capabilities2_runner_nav2/waypoint_runner.hpp b/capabilities2_runner_nav2/include/capabilities2_runner_nav2/waypoint_runner.hpp deleted file mode 100644 index bf2ad5c..0000000 --- a/capabilities2_runner_nav2/include/capabilities2_runner_nav2/waypoint_runner.hpp +++ /dev/null @@ -1,97 +0,0 @@ -#pragma once - -#include -#include -#include -#include -#include -#include - -#include -#include - -#include - -using namespace std::chrono_literals; - -namespace capabilities2_runner -{ - -/** - * @brief waypoint runner class - * - * Class to run waypointfollower action based capability - * - */ -class WayPointRunner : public ActionRunner -{ -public: - WayPointRunner() : ActionRunner() - { - } - - /** - * @brief Starter function for starting the action runner - * - * @param node shared pointer to the capabilities node. Allows to use ros node related functionalities - * @param run_config runner configuration loaded from the yaml file - */ - virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config) override - { - init_action(node, run_config, "navigate_to_pose"); - } - -protected: - /** - * @brief This generate goal function overrides the generate_goal() function from ActionRunner() - * @param parameters XMLElement that contains parameters in the format - '' - * @return ActionT::Goal the generated goal - */ - virtual nav2_msgs::action::NavigateToPose::Goal generate_goal(tinyxml2::XMLElement* parameters, int id) override - { - parameters->QueryDoubleAttribute("x", &x); - parameters->QueryDoubleAttribute("y", &y); - - info_("goal consist of x: " + std::to_string(x) + " and y: " + std::to_string(y), id); - - nav2_msgs::action::NavigateToPose::Goal goal_msg; - geometry_msgs::msg::PoseStamped pose_msg; - - pose_msg.header.frame_id = "map"; - pose_msg.header.stamp.sec = 0; - pose_msg.header.stamp.nanosec = 0; - - pose_msg.pose.position.x = x; - pose_msg.pose.position.y = y; - pose_msg.pose.position.z = 0.0; - pose_msg.pose.orientation.w = 1.0; // Set default orientation (facing forward) - - goal_msg.pose = pose_msg; - - return goal_msg; - } - - /** - * @brief This generate feedback function overrides the generate_feedback() function from ActionRunner() - * - * @param msg feedback message from the action server - * @return std::string of feedback information - */ - virtual std::string - generate_feedback(const typename nav2_msgs::action::NavigateToPose::Feedback::ConstSharedPtr msg) override - { - // std::string feedback = "x: " + std::to_string(msg->current_pose.pose.position.x) + - // " y: " + std::to_string(msg->current_pose.pose.position.y); - // return feedback; - return ""; - } - -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/package.xml b/capabilities2_runner_nav2/package.xml deleted file mode 100644 index 7222b62..0000000 --- a/capabilities2_runner_nav2/package.xml +++ /dev/null @@ -1,34 +0,0 @@ - - - capabilities2_runner_nav2 - 0.0.0 - TODO: Package description - - Kalana Ratnayake - Kalana Ratnayake - - Kalana Ratnayake - - MIT - - ament_cmake - - rclcpp - rclcpp_action - pluginlib - std_msgs - nav2_msgs - geometry_msgs - capabilities2_msgs - capabilities2_runner - event_logger - event_logger_msgs - tinyxml2_vendor - - ament_lint_auto - ament_lint_common - - - ament_cmake - - diff --git a/capabilities2_runner_nav2/plugins.xml b/capabilities2_runner_nav2/plugins.xml deleted file mode 100644 index 2c4d4ca..0000000 --- a/capabilities2_runner_nav2/plugins.xml +++ /dev/null @@ -1,17 +0,0 @@ - - - - A plugin that provide occupancy grid map extraction capability - - - - - A plugin that provide robot pose extraction capability - - - - - A plugin that provide nav2 waypoint navigation capability - - - diff --git a/capabilities2_runner_nav2/src/nav2_runners.cpp b/capabilities2_runner_nav2/src/nav2_runners.cpp deleted file mode 100644 index d0ea3f1..0000000 --- a/capabilities2_runner_nav2/src/nav2_runners.cpp +++ /dev/null @@ -1,10 +0,0 @@ -#include -#include -#include -#include -#include - -// register runner plugins -PLUGINLIB_EXPORT_CLASS(capabilities2_runner::WayPointRunner, capabilities2_runner::RunnerBase) -PLUGINLIB_EXPORT_CLASS(capabilities2_runner::RobotPoseRunner, capabilities2_runner::RunnerBase) -PLUGINLIB_EXPORT_CLASS(capabilities2_runner::OccupancyGridRunner, capabilities2_runner::RunnerBase) diff --git a/capabilities2_runner_prompt/.clang-format b/capabilities2_runner_prompt/.clang-format deleted file mode 100644 index d36804f..0000000 --- a/capabilities2_runner_prompt/.clang-format +++ /dev/null @@ -1,64 +0,0 @@ - -BasedOnStyle: Google -AccessModifierOffset: -2 -ConstructorInitializerIndentWidth: 2 -AlignEscapedNewlinesLeft: false -AlignTrailingComments: true -AllowAllParametersOfDeclarationOnNextLine: false -AllowShortIfStatementsOnASingleLine: false -AllowShortLoopsOnASingleLine: false -AllowShortFunctionsOnASingleLine: None -AlwaysBreakTemplateDeclarations: true -AlwaysBreakBeforeMultilineStrings: false -BreakBeforeBinaryOperators: false -BreakBeforeTernaryOperators: false -BreakConstructorInitializersBeforeComma: true -BinPackParameters: true -ColumnLimit: 120 -ConstructorInitializerAllOnOneLineOrOnePerLine: true -DerivePointerBinding: false -PointerBindsToType: true -ExperimentalAutoDetectBinPacking: false -IndentCaseLabels: true -MaxEmptyLinesToKeep: 1 -NamespaceIndentation: None -ObjCSpaceBeforeProtocolList: true -PenaltyBreakBeforeFirstCallParameter: 19 -PenaltyBreakComment: 60 -PenaltyBreakString: 1 -PenaltyBreakFirstLessLess: 1000 -PenaltyExcessCharacter: 1000 -PenaltyReturnTypeOnItsOwnLine: 90 -SpacesBeforeTrailingComments: 2 -Cpp11BracedListStyle: false -Standard: Auto -IndentWidth: 2 -TabWidth: 2 -UseTab: Never -IndentFunctionDeclarationAfterType: false -SpacesInParentheses: false -SpacesInAngles: false -SpaceInEmptyParentheses: false -SpacesInCStyleCastParentheses: false -SpaceAfterControlStatementKeyword: true -SpaceBeforeAssignmentOperators: true -ContinuationIndentWidth: 4 -SortIncludes: false -SpaceAfterCStyleCast: false - -# Configure each individual brace in BraceWrapping -BreakBeforeBraces: Custom - -# Control of individual brace wrapping cases -BraceWrapping: { - AfterClass: 'true' - AfterControlStatement: 'true' - AfterEnum : 'true' - AfterFunction : 'true' - AfterNamespace : 'true' - AfterStruct : 'true' - AfterUnion : 'true' - BeforeCatch : 'true' - BeforeElse : 'true' - IndentBraces : 'false' -} diff --git a/capabilities2_runner_prompt/CMakeLists.txt b/capabilities2_runner_prompt/CMakeLists.txt deleted file mode 100644 index 8df7df5..0000000 --- a/capabilities2_runner_prompt/CMakeLists.txt +++ /dev/null @@ -1,63 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(capabilities2_runner_prompt) - -# Default to C++17 -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) -endif() - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -find_package(ament_cmake REQUIRED) -find_package(rclcpp REQUIRED) -find_package(rclcpp_action REQUIRED) -find_package(pluginlib REQUIRED) -find_package(prompt_msgs REQUIRED) -find_package(capabilities2_msgs REQUIRED) -find_package(capabilities2_runner REQUIRED) -find_package(capabilities2_utils REQUIRED) -find_package(event_logger REQUIRED) -find_package(event_logger_msgs REQUIRED) -find_package(tinyxml2_vendor REQUIRED) -find_package(TinyXML2 REQUIRED) # provided by tinyxml2 upstream, or tinyxml2_vendor - -include_directories( - include -) - -add_library(${PROJECT_NAME} SHARED - src/prompt_runners.cpp -) - -ament_target_dependencies(${PROJECT_NAME} - rclcpp - rclcpp_action - pluginlib - prompt_msgs - capabilities2_msgs - capabilities2_runner - capabilities2_utils - event_logger - event_logger_msgs - TinyXML2 -) - -pluginlib_export_plugin_description_file(capabilities2_runner plugins.xml) - -install(TARGETS ${PROJECT_NAME} - EXPORT ${PROJECT_NAME} - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION bin -) - -install(DIRECTORY include/ - DESTINATION include -) - -ament_export_include_directories(include) -ament_export_libraries(${PROJECT_NAME}) - -ament_package() diff --git a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_capability_runner.hpp b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_capability_runner.hpp deleted file mode 100644 index 4bb4673..0000000 --- a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_capability_runner.hpp +++ /dev/null @@ -1,44 +0,0 @@ -#pragma once -#include -#include -#include -#include - -namespace capabilities2_runner -{ - -/** - * @brief prompt capabilities runner - * - * This class is a wrapper around the capabilities2 service runner and is used to pass - * data to prompt_tools/prompt service, providing it as a capability that prompts - * robot capabilities. - */ -class PromptCapabilityRunner : public PromptServiceRunner -{ -public: - PromptCapabilityRunner() : PromptServiceRunner() - { - } - - /** - * @brief generate the prompt used for prompting the capabilities. - * - * @param parameters tinyXML2 parameters - * @return std::string - */ - virtual void generate_prompt(tinyxml2::XMLElement* parameters, int id, std::string& prompt, bool& flush) override - { - tinyxml2::XMLElement* capabilitySpecsElement = parameters->FirstChildElement("CapabilitySpecs"); - - tinyxml2::XMLPrinter printer; - capabilitySpecsElement->Accept(&printer); - - std::string data(printer.CStr()); - - prompt = "The capabilities of the robot are given as follows" + data; - flush = false; - } -}; - -} // namespace capabilities2_runner diff --git a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_occupancy_runner.hpp b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_occupancy_runner.hpp deleted file mode 100644 index 529773c..0000000 --- a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_occupancy_runner.hpp +++ /dev/null @@ -1,47 +0,0 @@ -#pragma once -#include -#include -#include -#include - -namespace capabilities2_runner -{ -/** - * @brief prompt capability runner - * - * This class is a wrapper around the capabilities2 service runner and is used to - * call on the prompt_tools/prompt service, providing it as a capability that prompts - * occupancy grid map values - */ -class PromptOccupancyRunner : public PromptServiceRunner -{ -public: - PromptOccupancyRunner() : PromptServiceRunner() - { - } - - /** - * @brief generate the prompt used for prompting the occupancy grids. - * - * @param parameters tinyXML2 parameters - * @return std::string - */ - virtual void generate_prompt(tinyxml2::XMLElement* parameters, int id, std::string& prompt, bool& flush) override - { - tinyxml2::XMLElement* occupancyElement = parameters->FirstChildElement("OccupancyGrid"); - - tinyxml2::XMLPrinter printer; - occupancyElement->Accept(&printer); - - std::string data(printer.CStr()); - - prompt = "The OccupancyGrid of the robot shows the surrounding environment of the robot. The data " - "is given as a ros2 nav_msgs::msg::OccupancyGrid of which the content are " + - data; - flush = false; - - info_("prompting with : " + prompt, id); - } -}; - -} // namespace capabilities2_runner diff --git a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_runner.hpp b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_runner.hpp deleted file mode 100644 index e9b3460..0000000 --- a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_runner.hpp +++ /dev/null @@ -1,93 +0,0 @@ -#pragma once -#include -#include -#include -#include - -namespace capabilities2_runner -{ -/** - * @brief prompt capability runner - * - * This class is a wrapper around the capabilities2 service runner and is used to - * call on the prompt_tools/prompt service, providing it as a capability that prompts - * capabilitie plans values - */ -class PromptPlanRunner : public PromptServiceRunner -{ -public: - PromptPlanRunner() : PromptServiceRunner() - { - } - - /** - * @brief generate the prompt used for prompting the occupancy grids. - * - * @param parameters tinyXML2 parameters - * @return std::string - */ - virtual void generate_prompt(tinyxml2::XMLElement* parameters, int id, std::string& prompt, bool& flush) override - { - bool replan; - const char* task; - std::string taskString; - - parameters->QueryBoolAttribute("replan", &replan); - parameters->QueryStringAttribute("task", &task); - - if (task) - taskString = task; - else - taskString = ""; - - if (!replan) - { - prompt = "Build a xml plan based on the availbale capabilities to acheive mentioned task of " + taskString + - ". Return only the xml plan without explanations or comments."; - - flush = true; - } - else - { - tinyxml2::XMLElement* failedElements = parameters->FirstChildElement("FailedElements"); - - prompt = "Rebuild the xml plan based on the availbale capabilities to acheive mentioned task of " + taskString + - ". Just give the xml plan without explanations or comments. These XML " - "elements had incompatibilities. " + - std::string(failedElements->GetText()) + "Recorrect them as well"; - flush = true; - } - - info_("prompting with : " + prompt, id); - } - - /** - * @brief Update on_success event parameters with new data if avaible. - * - * This function is used to inject new data into the XMLElement containing - * parameters related to the on_success trigger event - * - * A pattern needs to be implemented in the derived class - * - * @param parameters pointer to the XMLElement containing parameters - * @return pointer to the XMLElement containing updated parameters - */ - virtual std::string update_on_success(std::string& parameters) - { - tinyxml2::XMLElement* element = convert_to_xml(parameters); - - std::string document_string = response_->response.response; - - // Create the plan element as a child of the existing parameters element - tinyxml2::XMLElement* textElement = element->GetDocument()->NewElement("ReceievdPlan"); - element->InsertEndChild(textElement); - textElement->SetText(document_string.c_str()); - - // Return the updated parameters element with Pose added - std::string result = convert_to_string(element); - - return result; - }; -}; - -} // namespace capabilities2_runner diff --git a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_pose_runner.hpp b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_pose_runner.hpp deleted file mode 100644 index 4c831d9..0000000 --- a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_pose_runner.hpp +++ /dev/null @@ -1,46 +0,0 @@ -#pragma once -#include -#include -#include -#include - -namespace capabilities2_runner -{ -/** - * @brief prompt pose runner - * - * This class is a wrapper around the capabilities2 service runner and is used to pass - * data to prompt_tools/prompt service, providing it as a capability that prompts - * robot pose values - */ -class PromptPoseRunner : public PromptServiceRunner -{ -public: - PromptPoseRunner() : PromptServiceRunner() - { - } - - /** - * @brief generate the prompt used for prompting the capabilities. - * - * @param parameters tinyXML2 parameters - * @return std::string - */ - virtual void generate_prompt(tinyxml2::XMLElement* parameters, int id, std::string& prompt, bool& flush) override - { - tinyxml2::XMLElement* poseElement = parameters->FirstChildElement("Pose"); - - tinyxml2::XMLPrinter printer; - poseElement->Accept(&printer); - - std::string data(printer.CStr()); - - prompt = "The position of the robot is given as a standard ros2 geometry_msgs::msg::Pose of which the content " - "are " + data; - flush = false; - - info_("prompting with : " + prompt, id); - } -}; - -} // namespace capabilities2_runner diff --git a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_service_runner.hpp b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_service_runner.hpp deleted file mode 100644 index a60ba9f..0000000 --- a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_service_runner.hpp +++ /dev/null @@ -1,91 +0,0 @@ -#pragma once -#include -#include -#include -#include -#include -#include - -namespace capabilities2_runner -{ -/** - * @brief prompt service runner - * - * This class is a base class and a wrapper around the capabilities2 service runner - * and is used to call on the prompt_tools/prompt service, providing it as a capability - * that prompts capabilitie plans values - */ -class PromptServiceRunner : public ServiceRunner -{ -public: - PromptServiceRunner() : ServiceRunner() - { - } - - /** - * @brief Starter function for starting the service runner - * - * @param node shared pointer to the capabilities node. Allows to use ros node related functionalities - * @param run_config runner configuration loaded from the yaml file - */ - virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config) override - { - init_service(node, run_config, "/prompt_bridge/prompt"); - } - -protected: - /** - * @brief Generate a request from parameters given. - * - * This function is used in conjunction with the trigger function to inject type erased parameters - * into the typed action - * - * A pattern needs to be implemented in the derived class - * - * @param parameters - * @return prompt_msgs::srv::Prompt::Request the generated request - */ - virtual typename prompt_msgs::srv::Prompt::Request generate_request(tinyxml2::XMLElement* parameters, int id) override - { - prompt_msgs::srv::Prompt::Request request; - - prompt_msgs::msg::ModelOption modelOption1; - modelOption1.key = "model"; - modelOption1.value = "llama3.2"; - - request.prompt.options.push_back(modelOption1); - - prompt_msgs::msg::ModelOption modelOption2; - modelOption2.key = "stream"; - modelOption2.value = false; - modelOption2.type = prompt_msgs::msg::ModelOption::BOOL_TYPE; - - request.prompt.options.push_back(modelOption2); - - generate_prompt(parameters, id, request.prompt.prompt, request.prompt.flush); - - return request; - } - - /** - * @brief generate the prompt used for prompting the data. - * - * @param parameters tinyXML2 parameters - * @return std::string - */ - virtual void generate_prompt(tinyxml2::XMLElement* parameters, int id, std::string& prompt, bool& flush) = 0; - - virtual void process_response(typename prompt_msgs::srv::Prompt::Response::SharedPtr response, int id) - { - if (response->response.buffered) - { - info_("information buffered", id); - } - else - { - info_("response received : " + response->response.response, id); - } - } -}; - -} // namespace capabilities2_runner diff --git a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_text_runner.hpp b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_text_runner.hpp deleted file mode 100644 index 026f70b..0000000 --- a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_text_runner.hpp +++ /dev/null @@ -1,45 +0,0 @@ -#pragma once -#include -#include -#include -#include - -namespace capabilities2_runner -{ -/** - * @brief prompt capability runner - * - * This class is a wrapper around the capabilities2 service runner and is used to - * call on the prompt_tools/prompt service, providing it as a capability that prompts - * text values - */ -class PromptTextRunner : public PromptServiceRunner -{ -public: - PromptTextRunner() : PromptServiceRunner() - { - } - - /** - * @brief generate the prompt used for prompting the text. - * - * @param parameters tinyXML2 parameters - * @return std::string - */ - virtual void generate_prompt(tinyxml2::XMLElement* parameters, int id, std::string& prompt, bool& flush) override - { - tinyxml2::XMLElement* textElement = parameters->FirstChildElement("Text"); - - tinyxml2::XMLPrinter printer; - textElement->Accept(&printer); - - std::string data(printer.CStr()); - - prompt = "The response was " + data; - flush = true; - - info_("prompting with : " + prompt, id); - } -}; - -} // namespace capabilities2_runner diff --git a/capabilities2_runner_prompt/package.xml b/capabilities2_runner_prompt/package.xml deleted file mode 100644 index 54160c3..0000000 --- a/capabilities2_runner_prompt/package.xml +++ /dev/null @@ -1,33 +0,0 @@ - - - - capabilities2_runner_prompt - 0.0.0 - TODO: Package description - - Kalana Ratnayake - Kalana Ratnayake - - Kalana Ratnayake - - TODO: License declaration - - ament_cmake - - rclcpp - rclcpp_action - pluginlib - prompt_msgs - capabilities2_msgs - capabilities2_runner - event_logger - event_logger_msgs - tinyxml2_vendor - - ament_lint_auto - ament_lint_common - - - ament_cmake - - diff --git a/capabilities2_runner_prompt/plugins.xml b/capabilities2_runner_prompt/plugins.xml deleted file mode 100644 index 343617d..0000000 --- a/capabilities2_runner_prompt/plugins.xml +++ /dev/null @@ -1,27 +0,0 @@ - - - - A plugin that provide capability prompting capability - - - - - A plugin that provide occupancy grid map prompting capability - - - - - A plugin that requests a new pplan based on available capabilities and a defined task - - - - - A plugin that provide pose prompting capability - - - - - A plugin that provide text prompting capability - - - diff --git a/capabilities2_runner_prompt/src/prompt_runners.cpp b/capabilities2_runner_prompt/src/prompt_runners.cpp deleted file mode 100644 index 1ebcd4d..0000000 --- a/capabilities2_runner_prompt/src/prompt_runners.cpp +++ /dev/null @@ -1,14 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include - -// register runner plugins -PLUGINLIB_EXPORT_CLASS(capabilities2_runner::PromptTextRunner, capabilities2_runner::RunnerBase) -PLUGINLIB_EXPORT_CLASS(capabilities2_runner::PromptPoseRunner, capabilities2_runner::RunnerBase) -PLUGINLIB_EXPORT_CLASS(capabilities2_runner::PromptOccupancyRunner, capabilities2_runner::RunnerBase) -PLUGINLIB_EXPORT_CLASS(capabilities2_runner::PromptPlanRunner, capabilities2_runner::RunnerBase) -PLUGINLIB_EXPORT_CLASS(capabilities2_runner::PromptCapabilityRunner, capabilities2_runner::RunnerBase) \ No newline at end of file diff --git a/capabilities2_runner_system/CMakeLists.txt b/capabilities2_runner_system/CMakeLists.txt index 9d56357..aaec417 100644 --- a/capabilities2_runner_system/CMakeLists.txt +++ b/capabilities2_runner_system/CMakeLists.txt @@ -57,6 +57,14 @@ install(DIRECTORY include/ DESTINATION include ) +install(DIRECTORY interfaces + DESTINATION share/${PROJECT_NAME} +) + +install(DIRECTORY providers + DESTINATION share/${PROJECT_NAME} +) + ament_export_include_directories(include) ament_export_libraries(${PROJECT_NAME}) diff --git a/system_capabilities/interfaces/CompletionRunner.yaml b/capabilities2_runner_system/interfaces/CompletionRunner.yaml similarity index 100% rename from system_capabilities/interfaces/CompletionRunner.yaml rename to capabilities2_runner_system/interfaces/CompletionRunner.yaml diff --git a/system_capabilities/interfaces/InputMultiplexAllRunner.yaml b/capabilities2_runner_system/interfaces/InputMultiplexAllRunner.yaml similarity index 100% rename from system_capabilities/interfaces/InputMultiplexAllRunner.yaml rename to capabilities2_runner_system/interfaces/InputMultiplexAllRunner.yaml diff --git a/system_capabilities/interfaces/InputMultiplexAnyRunner.yaml b/capabilities2_runner_system/interfaces/InputMultiplexAnyRunner.yaml similarity index 100% rename from system_capabilities/interfaces/InputMultiplexAnyRunner.yaml rename to capabilities2_runner_system/interfaces/InputMultiplexAnyRunner.yaml diff --git a/capabilities2_runner_system/package.xml b/capabilities2_runner_system/package.xml index dd79d51..f6449b3 100644 --- a/capabilities2_runner_system/package.xml +++ b/capabilities2_runner_system/package.xml @@ -32,5 +32,33 @@ ament_cmake + + + + interfaces/CompletionRunner.yaml + + + + providers/CompletionRunner.yaml + + + + + interfaces/InputMultiplexAllRunner.yaml + + + + providers/InputMultiplexAllRunner.yaml + + + + + interfaces/InputMultiplexAnyRunner.yaml + + + + providers/InputMultiplexAnyRunner.yaml + + diff --git a/system_capabilities/providers/CompletionRunner.yaml b/capabilities2_runner_system/providers/CompletionRunner.yaml similarity index 100% rename from system_capabilities/providers/CompletionRunner.yaml rename to capabilities2_runner_system/providers/CompletionRunner.yaml diff --git a/system_capabilities/providers/InputMultiplexAllRunner.yaml b/capabilities2_runner_system/providers/InputMultiplexAllRunner.yaml similarity index 100% rename from system_capabilities/providers/InputMultiplexAllRunner.yaml rename to capabilities2_runner_system/providers/InputMultiplexAllRunner.yaml diff --git a/system_capabilities/providers/InputMultiplexAnyRunner.yaml b/capabilities2_runner_system/providers/InputMultiplexAnyRunner.yaml similarity index 100% rename from system_capabilities/providers/InputMultiplexAnyRunner.yaml rename to capabilities2_runner_system/providers/InputMultiplexAnyRunner.yaml diff --git a/system_capabilities/CMakeLists.txt b/system_capabilities/CMakeLists.txt deleted file mode 100644 index 1d10d3b..0000000 --- a/system_capabilities/CMakeLists.txt +++ /dev/null @@ -1,16 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(system_capabilities) - -find_package(ament_cmake REQUIRED) - -# install interface files -install(DIRECTORY interfaces - DESTINATION share/${PROJECT_NAME} -) - -# install semantic interface files -install(DIRECTORY providers - DESTINATION share/${PROJECT_NAME} -) - -ament_package() diff --git a/system_capabilities/LICENSE b/system_capabilities/LICENSE deleted file mode 100644 index 30e8e2e..0000000 --- a/system_capabilities/LICENSE +++ /dev/null @@ -1,17 +0,0 @@ -Permission is hereby granted, free of charge, to any person obtaining a copy -of this software and associated documentation files (the "Software"), to deal -in the Software without restriction, including without limitation the rights -to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -copies of the Software, and to permit persons to whom the Software is -furnished to do so, subject to the following conditions: - -The above copyright notice and this permission notice shall be included in -all copies or substantial portions of the Software. - -THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL -THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -THE SOFTWARE. diff --git a/system_capabilities/package.xml b/system_capabilities/package.xml deleted file mode 100644 index 6bbf60c..0000000 --- a/system_capabilities/package.xml +++ /dev/null @@ -1,48 +0,0 @@ - - - - system_capabilities - 0.0.0 - TODO: Package description - kalana - MIT - - ament_cmake - - ament_lint_auto - ament_lint_common - - - ament_cmake - - - - interfaces/CompletionRunner.yaml - - - - providers/CompletionRunner.yaml - - - - - interfaces/InputMultiplexAllRunner.yaml - - - - providers/InputMultiplexAllRunner.yaml - - - - - - interfaces/InputMultiplexAnyRunner.yaml - - - - providers/InputMultiplexAnyRunner.yaml - - - - - From c554ebf5f9b9680c9243c7fef383403d2f9cb3ad Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Fri, 22 Aug 2025 15:23:01 +1000 Subject: [PATCH 127/133] updated documentation --- docs/fabric_setup.md | 25 +++++++++++++++++++++++++ docs/nav2_setup.md | 17 +++++++++-------- docs/prompt_tools_setup.md | 9 ++++++++- readme.md | 1 + 4 files changed, 43 insertions(+), 9 deletions(-) create mode 100644 docs/fabric_setup.md diff --git a/docs/fabric_setup.md b/docs/fabric_setup.md new file mode 100644 index 0000000..7ce2a50 --- /dev/null +++ b/docs/fabric_setup.md @@ -0,0 +1,25 @@ +# Dependency installation for Prompt Tools Runners + +## Clone packages + +Clone the fabric package same workspace if its not already availabe in the workspace. Capabilities2 Fabric Runners are dependent on this package. + +```bash +cd src +git clone https://github.com/CollaborativeRoboticsLab/fabric.git +``` + +## Clone Capabilities2 plugin for Fabric stack + +```bash +cd src +git clone https://github.com/CollaborativeRoboticsLab/fabric.git +``` + +## Dependency Installation + +Move to workspace root and run the following command to install dependencies + +```bash +rosdep install --from-paths src --ignore-src -r -y +``` \ No newline at end of file diff --git a/docs/nav2_setup.md b/docs/nav2_setup.md index 2a44385..b8eaab5 100644 --- a/docs/nav2_setup.md +++ b/docs/nav2_setup.md @@ -1,26 +1,27 @@ # Dependency installation for Nav2 Runners -## Install nav2 stack +## Install nav2 stack and slam-toolbox Run the following commands to install nav2 stack ```bash -sudo apt install ros-humble-navigation2 ros-humble-nav2-bringup +sudo apt install ros-$ROS_DISTRO-navigation2 ros-$ROS_DISTRO-nav2-bringup ros-$ROS_DISTRO-slam-toolbox ``` -## Install slam-toolbox +## Clone Nav2 configuration + +Clone the nav_stack default configuration and launch files to the same workspace if its not already availabe in the workspace. Capabilities2 Nav2 Runners are dependent on this package. ```bash -sudo apt install ros-$ROS_DISTRO-slam-toolbox +cd src +git clone https://github.com/CollaborativeRoboticsLab/nav_stack.git ``` -## Clone configuration - -Clone the nav_stack default configuration and launch files to the same workspace if its not already availabe in the workspace. Capabilities2 Nav2 Runners are dependent on this package. +## Clone Capabilities2 plugin for Nav2 stack ```bash cd src -git clone https://github.com/CollaborativeRoboticsLab/nav_stack.git +git clone https://github.com/CollaborativeRoboticsLab/capabilities2_runner_nav2.git ``` ## Turtlebot3 Simulation (Optional) diff --git a/docs/prompt_tools_setup.md b/docs/prompt_tools_setup.md index 466c21e..dd71076 100644 --- a/docs/prompt_tools_setup.md +++ b/docs/prompt_tools_setup.md @@ -6,7 +6,14 @@ Clone the prompt tools package same workspace if its not already availabe in the ```bash cd src -git clone https://github.com/CollaborativeRoboticsLab/prompt_tools.git -b develop +git clone https://github.com/CollaborativeRoboticsLab/prompt_tools.git +``` + +## Clone Capabilities2 plugin for Prompt tools stack + +```bash +cd src +git clone https://github.com/CollaborativeRoboticsLab/capabilities2_runner_prompt.git ``` ## Dependency Installation diff --git a/readme.md b/readme.md index 99ac6dc..139ce28 100644 --- a/readme.md +++ b/readme.md @@ -37,6 +37,7 @@ Runners can be created using the runner API parent classes [here](./capabilities - [Setup Instructions without devcontainer](./docs/setup.md) - [Dependency installation for Nav2 Runners](./docs/nav2_setup.md) - [Dependency installation for Prompt Runners](./docs/prompt_tools_setup.md) +- [Dependency installation for Fabric Runners](./docs/fabric_setup.md) - [Dependency installation for Foxglove-studio](./docs/foxglove_studio.md) ## Quick Startup information From 92662a26e36783870776f64052a56cef1f719040 Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Fri, 22 Aug 2025 23:12:12 +1000 Subject: [PATCH 128/133] minor fixes --- docs/setup.md | 24 +++++++++++------------- readme.md | 6 +++--- 2 files changed, 14 insertions(+), 16 deletions(-) diff --git a/docs/setup.md b/docs/setup.md index 74bce4a..05ac7d6 100644 --- a/docs/setup.md +++ b/docs/setup.md @@ -11,26 +11,18 @@ mkdir -p /home/$USER/capabilities_ws/src cd /home/$USER/capabilities_ws/src ``` -### Tinyxml2 Installation - -Clone and install the tinyXML2 package - -```bash -git clone https://github.com/leethomason/tinyxml2.git -cd tinyxml2 -make install -``` - ### Cloning the Packages Clone the package using Git ```bash -git clone -b capabilities2-server-fabric https://github.com/CollaborativeRoboticsLab/capabilities2.git -git clone -b develop https://github.com/CollaborativeRoboticsLab/std_capabilities.git -git clone https://github.com/AIResearchLab/nav_stack.git +git clone https://github.com/CollaborativeRoboticsLab/capabilities2.git ``` +Optionally you can clone +```bash +git https://github.com/CollaborativeRoboticsLab/std_capabilities.git +``` ### Dependency installation Move the terminal to workspace root and install dependencies. @@ -49,3 +41,9 @@ Use colcon to build the packages: ```bash colcon build ``` + +### Additional installtions + +[Fabric](fabric_setup.md) \ +[Prompt Tools](./prompt_tools_setup.md) \ +[Nav2 stack](./nav2_setup.md) \ No newline at end of file diff --git a/readme.md b/readme.md index 139ce28..e7bacbd 100644 --- a/readme.md +++ b/readme.md @@ -1,4 +1,4 @@ -# capabilities2 +# Capabilities2 [![ROS2 Jazzy](https://img.shields.io/badge/ROS2-Jazzy-blue)](https://index.ros.org/doc/ros2/Releases/) [![License: MIT](https://img.shields.io/badge/License-MIT-yellow.svg)](https://opensource.org/licenses/MIT) @@ -33,11 +33,11 @@ Runners can be created using the runner API parent classes [here](./capabilities ## Setup Information +- [Installation](./docs/setup.md) - [Setup Instructions with devcontainer](./docs/setup_with_dev.md) -- [Setup Instructions without devcontainer](./docs/setup.md) +- [Dependency installation for Fabric Runners](./docs/fabric_setup.md) - [Dependency installation for Nav2 Runners](./docs/nav2_setup.md) - [Dependency installation for Prompt Runners](./docs/prompt_tools_setup.md) -- [Dependency installation for Fabric Runners](./docs/fabric_setup.md) - [Dependency installation for Foxglove-studio](./docs/foxglove_studio.md) ## Quick Startup information From 27893bc493b82d710df8724b1d0d5167a1031448 Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Sat, 23 Aug 2025 17:20:56 +1000 Subject: [PATCH 129/133] fixes to match code refactoring --- .../interfaces/CapabilityGetRunner.yaml | 10 ++-- .../providers/CapabilityGetRunner.yaml | 2 +- .../completion_runner.hpp | 52 ------------------- .../interfaces/CompletionRunner.yaml | 13 ----- capabilities2_runner_system/package.xml | 9 ---- capabilities2_runner_system/plugins.xml | 5 -- .../providers/CompletionRunner.yaml | 8 --- .../providers/InputMultiplexAllRunner.yaml | 4 +- .../providers/InputMultiplexAnyRunner.yaml | 4 +- .../src/capabilities2_runner.cpp | 4 +- 10 files changed, 11 insertions(+), 100 deletions(-) delete mode 100644 capabilities2_runner_system/include/capabilities2_runner_system/completion_runner.hpp delete mode 100644 capabilities2_runner_system/interfaces/CompletionRunner.yaml delete mode 100644 capabilities2_runner_system/providers/CompletionRunner.yaml diff --git a/capabilities2_runner_capabilities/interfaces/CapabilityGetRunner.yaml b/capabilities2_runner_capabilities/interfaces/CapabilityGetRunner.yaml index 79535fc..c50bbbb 100644 --- a/capabilities2_runner_capabilities/interfaces/CapabilityGetRunner.yaml +++ b/capabilities2_runner_capabilities/interfaces/CapabilityGetRunner.yaml @@ -3,11 +3,11 @@ name: CapabilityGetRunner spec_version: 1.1 spec_type: interface -description: "This capability depends on the capabilities2 server functionalities and allows an decision making authority such as an - LLM extract capabilities of the robot. The capability can be trigged with an command such as - ''. This is included in - the default starting plan and the decision making authority such as an LLM does not need to include this in any generated - plans. The details for the Capaiblities are stored in a format as follows, +description: "This capability depends on the capabilities2 server functionalities and allows an decision making authority such as an LLM extract + capabilities of the robot. The capability can be trigged with an command such as + ''. + This is included in the default starting plan and the decision making authority such as an LLM does not need to include this in any + generated plans. The details for the Capaiblities are stored in a format as follows, ' diff --git a/capabilities2_runner_capabilities/providers/CapabilityGetRunner.yaml b/capabilities2_runner_capabilities/providers/CapabilityGetRunner.yaml index 7202ee7..640e3fc 100644 --- a/capabilities2_runner_capabilities/providers/CapabilityGetRunner.yaml +++ b/capabilities2_runner_capabilities/providers/CapabilityGetRunner.yaml @@ -5,5 +5,5 @@ name: CapabilityGetRunner spec_type: provider spec_version: 1.1 description: "The capability provider for extracting capabilities from server. This is used to identify the capabilities of the robot" -implements: std_capabilities/CapabilityGetRunner +implements: capabilities2_runner_capabilities/CapabilityGetRunner runner: capabilities2_runner::CapabilityGetRunner \ No newline at end of file diff --git a/capabilities2_runner_system/include/capabilities2_runner_system/completion_runner.hpp b/capabilities2_runner_system/include/capabilities2_runner_system/completion_runner.hpp deleted file mode 100644 index 8e5dcdc..0000000 --- a/capabilities2_runner_system/include/capabilities2_runner_system/completion_runner.hpp +++ /dev/null @@ -1,52 +0,0 @@ -#pragma once -#include -#include -#include -#include -#include - -namespace capabilities2_runner -{ -/** - * @brief completion runner - * - * This class is a wrapper around the capabilities2 service runner and is used to - * call on the /fabric/set_completion service, providing it as a - * capability that notifys the completion of the fabric - */ -class CompletionRunner : public ServiceRunner -{ -public: - CompletionRunner() : ServiceRunner() - { - } - - /** - * @brief Starter function for starting the service runner - * - * @param node shared pointer to the capabilities node. Allows to use ros node related functionalities - * @param run_config runner configuration loaded from the yaml file - */ - virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config) override - { - init_service(node, run_config, "/fabric/set_completion"); - } - -protected: - /** - * @brief Generate a empty request. - * - * This function is used in conjunction with the trigger function to inject type erased parameters - * into the typed action - * - * @param parameters - * @return prompt_msgs::srv::Prompt::Request the generated request - */ - virtual typename fabric_msgs::srv::CompleteFabric::Request generate_request(tinyxml2::XMLElement* parameters, int id) override - { - fabric_msgs::srv::CompleteFabric::Request request; - return request; - } -}; - -} // namespace capabilities2_runner diff --git a/capabilities2_runner_system/interfaces/CompletionRunner.yaml b/capabilities2_runner_system/interfaces/CompletionRunner.yaml deleted file mode 100644 index d34d09f..0000000 --- a/capabilities2_runner_system/interfaces/CompletionRunner.yaml +++ /dev/null @@ -1,13 +0,0 @@ -%YAML 1.1 ---- -name: CompletionRunner -spec_version: 1.1 -spec_type: interface -description: "This capability notifies the completion of the capabilities fabric to the completion_server on the capabilities2 fabric - package. This is included on every fabric as the last capability to be triggered during connection idenification for the - fabric. A decision making authority such as an LLM does not need to include this in plans generated by it." -interface: - actions: - "/fabric/set_completion": - type: "capabilities2_msgs::srv::CompleteFabric" - description: "Fabric completion notifier interface of the Capabilities fabric" \ No newline at end of file diff --git a/capabilities2_runner_system/package.xml b/capabilities2_runner_system/package.xml index f6449b3..1af9a4e 100644 --- a/capabilities2_runner_system/package.xml +++ b/capabilities2_runner_system/package.xml @@ -33,15 +33,6 @@ ament_cmake - - - interfaces/CompletionRunner.yaml - - - - providers/CompletionRunner.yaml - - interfaces/InputMultiplexAllRunner.yaml diff --git a/capabilities2_runner_system/plugins.xml b/capabilities2_runner_system/plugins.xml index 41e8b96..bc05f3c 100644 --- a/capabilities2_runner_system/plugins.xml +++ b/capabilities2_runner_system/plugins.xml @@ -11,9 +11,4 @@ It can handle multiple input streams and route them to the appropriate output. - - - A plugin that notifies about the completion of the fabric to the action server - - diff --git a/capabilities2_runner_system/providers/CompletionRunner.yaml b/capabilities2_runner_system/providers/CompletionRunner.yaml deleted file mode 100644 index 3fb38e6..0000000 --- a/capabilities2_runner_system/providers/CompletionRunner.yaml +++ /dev/null @@ -1,8 +0,0 @@ -%YAML 1.1 ---- -name: CompletionRunner -spec_type: provider -spec_version: 1.1 -description: "The capability provider for the /fabric/set_completion interface" -implements: system_capabilities/CompletionRunner -runner: capabilities2_runner::CompletionRunner diff --git a/capabilities2_runner_system/providers/InputMultiplexAllRunner.yaml b/capabilities2_runner_system/providers/InputMultiplexAllRunner.yaml index b3edbfd..1720a2b 100644 --- a/capabilities2_runner_system/providers/InputMultiplexAllRunner.yaml +++ b/capabilities2_runner_system/providers/InputMultiplexAllRunner.yaml @@ -3,6 +3,6 @@ name: InputMultiplexAllRunner spec_type: provider spec_version: 1.1 -description: "The capability provider for the system_capabilities/InputMultiplexAllRunner interface" -implements: system_capabilities/InputMultiplexAllRunner +description: "The capability provider for the capabilities2_runner_system/InputMultiplexAllRunner interface" +implements: capabilities2_runner_system/InputMultiplexAllRunner runner: capabilities2_runner::InputMultiplexAllRunner \ No newline at end of file diff --git a/capabilities2_runner_system/providers/InputMultiplexAnyRunner.yaml b/capabilities2_runner_system/providers/InputMultiplexAnyRunner.yaml index 789ee28..9e07449 100644 --- a/capabilities2_runner_system/providers/InputMultiplexAnyRunner.yaml +++ b/capabilities2_runner_system/providers/InputMultiplexAnyRunner.yaml @@ -3,6 +3,6 @@ name: InputMultiplexAnyRunner spec_type: provider spec_version: 1.1 -description: "The capability provider for the system_capabilities/InputMultiplexAnyRunner interface" -implements: system_capabilities/InputMultiplexAnyRunner +description: "The capability provider for the capabilities2_runner_system/InputMultiplexAnyRunner interface" +implements: capabilities2_runner_system/InputMultiplexAnyRunner runner: capabilities2_runner::InputMultiplexAnyRunner \ No newline at end of file diff --git a/capabilities2_runner_system/src/capabilities2_runner.cpp b/capabilities2_runner_system/src/capabilities2_runner.cpp index 9d828da..114133e 100644 --- a/capabilities2_runner_system/src/capabilities2_runner.cpp +++ b/capabilities2_runner_system/src/capabilities2_runner.cpp @@ -1,10 +1,8 @@ #include #include -#include #include #include // register runner plugins PLUGINLIB_EXPORT_CLASS(capabilities2_runner::InputMultiplexAllRunner, capabilities2_runner::RunnerBase); -PLUGINLIB_EXPORT_CLASS(capabilities2_runner::InputMultiplexAnyRunner, capabilities2_runner::RunnerBase); -PLUGINLIB_EXPORT_CLASS(capabilities2_runner::CompletionRunner, capabilities2_runner::RunnerBase) \ No newline at end of file +PLUGINLIB_EXPORT_CLASS(capabilities2_runner::InputMultiplexAnyRunner, capabilities2_runner::RunnerBase); \ No newline at end of file From 201b9058f08c8c76712f858eee377a02f1bc5f3f Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Sun, 24 Aug 2025 00:47:30 +1000 Subject: [PATCH 130/133] minor fixes after refactoring --- .../include/capabilities2_runner/action_runner.hpp | 12 ++++++++++-- .../include/capabilities2_runner/runner_base.hpp | 2 +- .../include/capabilities2_runner/service_runner.hpp | 9 ++++++++- .../include/capabilities2_runner/topic_runner.hpp | 9 ++++++++- 4 files changed, 27 insertions(+), 5 deletions(-) diff --git a/capabilities2_runner/include/capabilities2_runner/action_runner.hpp b/capabilities2_runner/include/capabilities2_runner/action_runner.hpp index de2786a..7c768df 100644 --- a/capabilities2_runner/include/capabilities2_runner/action_runner.hpp +++ b/capabilities2_runner/include/capabilities2_runner/action_runner.hpp @@ -93,7 +93,8 @@ class ActionRunner : public RunnerBase // Trigger on_stopped event if defined if (events[runner_id].on_stopped.interface != "") { - event_(EventType::STOPPED, -1, events[runner_id].on_stopped.interface, events[runner_id].on_stopped.provider); + event_(EventType::STOPPED, -1, events[runner_id].on_stopped.interface, + events[runner_id].on_stopped.provider); triggerFunction_(events[runner_id].on_stopped.interface, update_on_stopped(events[runner_id].on_stopped.parameters)); } @@ -116,7 +117,14 @@ class ActionRunner : public RunnerBase } } - info_("stopping runner"); + info_("removing event options"); + + // remove all event options for this runner instance + const auto n = events.size(); + events.clear(); + info_("removed event options for " + std::to_string(n) + " runner ids"); + + info_("runner cleaned. stopping.."); } /** diff --git a/capabilities2_runner/include/capabilities2_runner/runner_base.hpp b/capabilities2_runner/include/capabilities2_runner/runner_base.hpp index 65d8659..b0e20ae 100644 --- a/capabilities2_runner/include/capabilities2_runner/runner_base.hpp +++ b/capabilities2_runner/include/capabilities2_runner/runner_base.hpp @@ -115,7 +115,7 @@ class RunnerBase parameters_[runner_id] = element; - info_("received new parameters with event id", runner_id); + info_("received new parameters with event id : " + std::to_string(runner_id), runner_id); executionThreadPool[runner_id] = std::thread(&RunnerBase::execution, this, runner_id); diff --git a/capabilities2_runner/include/capabilities2_runner/service_runner.hpp b/capabilities2_runner/include/capabilities2_runner/service_runner.hpp index 5238c0b..d28f6be 100644 --- a/capabilities2_runner/include/capabilities2_runner/service_runner.hpp +++ b/capabilities2_runner/include/capabilities2_runner/service_runner.hpp @@ -143,7 +143,14 @@ class ServiceRunner : public RunnerBase update_on_stopped(events[runner_id].on_stopped.parameters)); } - info_("stopping runner"); + info_("removing event options"); + + // remove all event options for this runner instance + const auto n = events.size(); + events.clear(); + info_("removed event options for " + std::to_string(n) + " runner ids"); + + info_("runner cleaned. stopping.."); } protected: diff --git a/capabilities2_runner/include/capabilities2_runner/topic_runner.hpp b/capabilities2_runner/include/capabilities2_runner/topic_runner.hpp index 0fb84ce..5cf2138 100644 --- a/capabilities2_runner/include/capabilities2_runner/topic_runner.hpp +++ b/capabilities2_runner/include/capabilities2_runner/topic_runner.hpp @@ -121,7 +121,14 @@ class TopicRunner : public RunnerBase update_on_stopped(events[runner_id].on_stopped.parameters)); } - info_("stopping runner"); + info_("removing event options"); + + // remove all event options for this runner instance + const auto n = events.size(); + events.clear(); + info_("removed event options for " + std::to_string(n) + " runner ids"); + + info_("runner cleaned. stopping.."); } protected: From 4e1f8eb58a9c910346e45e548519f0d359065cf4 Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Mon, 15 Sep 2025 12:01:40 +1000 Subject: [PATCH 131/133] minor readme fix --- docs/fabric_setup.md | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/docs/fabric_setup.md b/docs/fabric_setup.md index 7ce2a50..ae5f63a 100644 --- a/docs/fabric_setup.md +++ b/docs/fabric_setup.md @@ -1,8 +1,8 @@ -# Dependency installation for Prompt Tools Runners +# Dependency installation for Fabric Runners ## Clone packages -Clone the fabric package same workspace if its not already availabe in the workspace. Capabilities2 Fabric Runners are dependent on this package. +Clone the fabric package same workspace if its not already availabe in the workspace. Fabric Runners are dependent on this package. ```bash cd src @@ -13,7 +13,7 @@ git clone https://github.com/CollaborativeRoboticsLab/fabric.git ```bash cd src -git clone https://github.com/CollaborativeRoboticsLab/fabric.git +git clone https://github.com/CollaborativeRoboticsLab/capabilities2_runner_fabric.git ``` ## Dependency Installation From 2dfa4f8040489860be3d7b3c7dff01c46588a3c1 Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Tue, 18 Nov 2025 06:29:39 +0000 Subject: [PATCH 132/133] added humble devcontainer alongside jazzy --- .devcontainer/humble/Dockerfile | 30 +++++++++++++++++++++ .devcontainer/humble/devcontainer.json | 17 ++++++++++++ .devcontainer/{ => jazzy}/Dockerfile | 0 .devcontainer/{ => jazzy}/devcontainer.json | 0 4 files changed, 47 insertions(+) create mode 100644 .devcontainer/humble/Dockerfile create mode 100644 .devcontainer/humble/devcontainer.json rename .devcontainer/{ => jazzy}/Dockerfile (100%) rename .devcontainer/{ => jazzy}/devcontainer.json (100%) diff --git a/.devcontainer/humble/Dockerfile b/.devcontainer/humble/Dockerfile new file mode 100644 index 0000000..a71f968 --- /dev/null +++ b/.devcontainer/humble/Dockerfile @@ -0,0 +1,30 @@ +FROM ros:humble + +# Add vscode user with same UID and GID as your host system +# (modified from https://code.visualstudio.com/remote/advancedcontainers/add-nonroot-user#_creating-a-nonroot-user) +# ubuntu now has a 1000 user ubuntu +ARG USERNAME=ubuntu +ARG USER_UID=1000 +ARG USER_GID=$USER_UID + +RUN groupadd --gid $USER_GID $USERNAME \ + && useradd --uid $USER_UID --gid $USER_GID -m $USERNAME + +RUN apt-get update && apt-get upgrade -y \ + && apt-get install -y \ + sudo \ + git \ + && echo $USERNAME ALL=\(root\) NOPASSWD:ALL > /etc/sudoers.d/$USERNAME \ + && chmod 0440 /etc/sudoers.d/$USERNAME + +# Switch from root to user +USER $USERNAME + +# Rosdep update +RUN rosdep update + +# Source the ROS setup file +RUN echo "source /opt/ros/${ROS_DISTRO}/setup.bash" >> ~/.bashrc + +# make sure folders exist +RUN mkdir -p ~/colcon_ws/src diff --git a/.devcontainer/humble/devcontainer.json b/.devcontainer/humble/devcontainer.json new file mode 100644 index 0000000..518152d --- /dev/null +++ b/.devcontainer/humble/devcontainer.json @@ -0,0 +1,17 @@ +{ + "name": "humble base", + "dockerFile": "Dockerfile", + "remoteUser": "ubuntu", + "containerEnv": { + "DISPLAY": "${localEnv:DISPLAY}" + }, + "runArgs": [ + "--privileged", + "--network=host" + ], + "workspaceMount": "source=${localWorkspaceFolder},target=/home/ubuntu/colcon_ws/${localWorkspaceFolderBasename},type=bind", + "workspaceFolder": "/home/ubuntu/colcon_ws", + "mounts": [ + "source=${localEnv:HOME}${localEnv:USERPROFILE}/.bash_history,target=/home/ubuntu/.bash_history,type=bind" + ] +} diff --git a/.devcontainer/Dockerfile b/.devcontainer/jazzy/Dockerfile similarity index 100% rename from .devcontainer/Dockerfile rename to .devcontainer/jazzy/Dockerfile diff --git a/.devcontainer/devcontainer.json b/.devcontainer/jazzy/devcontainer.json similarity index 100% rename from .devcontainer/devcontainer.json rename to .devcontainer/jazzy/devcontainer.json From 39c93a27467eb50ced585b3cb9ba69231240b1fe Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Tue, 18 Nov 2025 06:31:47 +0000 Subject: [PATCH 133/133] added humble devcontainer alongside jazzy --- .devcontainer/humble/devcontainer.json | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.devcontainer/humble/devcontainer.json b/.devcontainer/humble/devcontainer.json index 518152d..02e20ce 100644 --- a/.devcontainer/humble/devcontainer.json +++ b/.devcontainer/humble/devcontainer.json @@ -9,7 +9,7 @@ "--privileged", "--network=host" ], - "workspaceMount": "source=${localWorkspaceFolder},target=/home/ubuntu/colcon_ws/${localWorkspaceFolderBasename},type=bind", + "workspaceMount": "source=${localWorkspaceFolder},target=/home/ubuntu/colcon_ws/src/${localWorkspaceFolderBasename},type=bind", "workspaceFolder": "/home/ubuntu/colcon_ws", "mounts": [ "source=${localEnv:HOME}${localEnv:USERPROFILE}/.bash_history,target=/home/ubuntu/.bash_history,type=bind"