From c51118f8923dec8fabc8924745db4c1b043b48a5 Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Thu, 3 Oct 2024 19:27:31 +1000 Subject: [PATCH 1/2] added executor, file parser and other support components --- capabilities2_executor/CMakeLists.txt | 15 + .../capabilities_executor.hpp | 411 ++++++++++++++---- .../capabilities_file_parser.hpp | 135 ++++++ .../capabilities_xml_parser.hpp | 168 ++++++- .../structs/connection.hpp | 27 ++ .../src/capabilities_file_parser.cpp | 9 + capabilities2_msgs/CMakeLists.txt | 2 + capabilities2_msgs/action/Connections.action | 13 + capabilities2_msgs/action/Plan.action | 1 + .../msg/CapabilityConnection.msg | 14 + .../capabilities2_runner/action_runner.hpp | 6 +- 11 files changed, 701 insertions(+), 100 deletions(-) create mode 100644 capabilities2_executor/include/capabilities2_executor/capabilities_file_parser.hpp create mode 100644 capabilities2_executor/include/capabilities2_executor/structs/connection.hpp create mode 100644 capabilities2_executor/src/capabilities_file_parser.cpp create mode 100644 capabilities2_msgs/action/Connections.action create mode 100644 capabilities2_msgs/msg/CapabilityConnection.msg diff --git a/capabilities2_executor/CMakeLists.txt b/capabilities2_executor/CMakeLists.txt index 16f5f19..ccec815 100644 --- a/capabilities2_executor/CMakeLists.txt +++ b/capabilities2_executor/CMakeLists.txt @@ -39,6 +39,21 @@ ament_target_dependencies(${PROJECT_NAME} TINYXML2 ) +add_library(${PROJECT_NAME}_file SHARED + src/capabilities_file_parser.cpp +) + +target_link_libraries(${PROJECT_NAME}_file + ${TINYXML2_LIBRARIES} +) + +ament_target_dependencies(${PROJECT_NAME}_file + rclcpp + rclcpp_action + capabilities2_msgs + TINYXML2 +) + install(DIRECTORY include/ DESTINATION include ) diff --git a/capabilities2_executor/include/capabilities2_executor/capabilities_executor.hpp b/capabilities2_executor/include/capabilities2_executor/capabilities_executor.hpp index 4def6b8..7b5b40b 100644 --- a/capabilities2_executor/include/capabilities2_executor/capabilities_executor.hpp +++ b/capabilities2_executor/include/capabilities2_executor/capabilities_executor.hpp @@ -10,10 +10,15 @@ #include +#include + #include +#include + #include #include #include +#include /** * @brief Capabilities Executor @@ -33,14 +38,6 @@ class CapabilitiesExecutor : public rclcpp::Node control_tag_list.push_back("sequential"); control_tag_list.push_back("parallel"); control_tag_list.push_back("recovery"); - control_tag_list.push_back("roundrobin"); - // control_tag_list.push_back("behaviourtree"); - - declare_parameter("read_file", false); - read_file = get_parameter("read_file").as_bool(); - - declare_parameter("plan_file_path", "plan.xml"); - plan_file_path = get_parameter("plan_file_path").as_string(); using namespace std::placeholders; @@ -51,26 +48,12 @@ class CapabilitiesExecutor : public rclcpp::Node std::bind(&CapabilitiesExecutor::handle_cancel, this, _1), std::bind(&CapabilitiesExecutor::handle_accepted, this, _1)); + this->client_capabilities_ = rclcpp_action::create_client(this, "~/capabilities_fabric"); + get_interfaces_client_ = this->create_client("~/get_interfaces"); get_sem_interf_client_ = this->create_client("~/get_semantic_interfaces"); establish_bond_client_ = this->create_client("~/establish_bond"); - - if (read_file) - { - // 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(); - } - - execution_thread = std::make_unique(std::bind(&CapabilitiesExecutor::execute_plan, this)); - } } private: @@ -100,6 +83,12 @@ class CapabilitiesExecutor : public rclcpp::Node 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"); @@ -108,20 +97,35 @@ class CapabilitiesExecutor : public rclcpp::Node return rclcpp_action::CancelResponse::ACCEPT; } + /** + * @brief Handle the goal accept event originating from handle_goal. + * + * @param goal_handle pointer to the action goal handle + */ void handle_accepted(const std::shared_ptr> goal_handle) { - execution_thread = std::make_unique(std::bind(&CapabilitiesExecutor::execute_plan, this)); + using namespace std::placeholders; + execution_thread = std::make_unique(std::bind(&CapabilitiesExecutor::execute_plan, this, _1), goal_handle); } - bool verify_plan() + /** + * @brief request the interfaces, semantic_interfaces and providers from the capabilities2 server + * + * @return `true` if interface retreival is successful,`false` otherwise + */ + bool request_information() { + // create request messages auto request_interface = std::make_shared(); auto request_sematic = std::make_shared(); + auto request_providers = std::make_shared(); + // request data from the server auto result_future = get_interfaces_client_->async_send_request(request_interface); RCLCPP_INFO(this->get_logger(), "Requesting Interface information from Capabilities2 Server"); + // wait until data is received if (rclcpp::spin_until_future_complete(shared_from_this(), result_future) != rclcpp::FutureReturnCode::SUCCESS) { RCLCPP_INFO(this->get_logger(), "Failed to receive Interface information from Capabilities2 Server"); @@ -130,15 +134,18 @@ class CapabilitiesExecutor : public rclcpp::Node RCLCPP_INFO(this->get_logger(), "Received Interface information from Capabilities2 Server"); + // request semantic interfaces available for each and every interface got from the server for (const auto &interface : result_future.get()->interfaces) { request_sematic->interface = interface; - auto result_remantic_future = get_sem_interf_client_->async_send_request(request_sematic); + // 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()); - if (rclcpp::spin_until_future_complete(shared_from_this(), result_remantic_future) != rclcpp::FutureReturnCode::SUCCESS) + // 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; @@ -146,113 +153,346 @@ class CapabilitiesExecutor : public rclcpp::Node RCLCPP_INFO(this->get_logger(), "Received Semantic Interface information from Capabilities2 Server for %s", interface.c_str()); - if (result_remantic_future.get()->semantic_interfaces.size() > 0) + // add sematic interfaces to the list if available + if (result_semantic_future.get()->semantic_interfaces.size() > 0) { - for (const auto &semantic_interface : result_remantic_future.get()->semantic_interfaces) + for (const auto &semantic_interface : result_semantic_future.get()->semantic_interfaces) { interface_list.push_back(semantic_interface); + + // request providers of the semantic interface + request_providers->interface = semantic_interface; + request_providers->include_semantic = true; + + auto result_providers_future = get_providers_client_->async_send_request(request_providers); + + RCLCPP_INFO(this->get_logger(), "Requesting Providers information from Capabilities2 Server for semantic interface %s", semantic_interface.c_str()); + + // wait until data is received + if (rclcpp::spin_until_future_complete(shared_from_this(), result_providers_future) != rclcpp::FutureReturnCode::SUCCESS) + { + RCLCPP_INFO(this->get_logger(), "Failed to receive Providers information from Capabilities2 Server for %s", semantic_interface.c_str()); + return false; + } + + RCLCPP_INFO(this->get_logger(), "Received Providers information from Capabilities2 Server for %s", semantic_interface.c_str()); + + // add defualt provider to the list + providers_list.push_back(result_providers_future.get()->default_provider); + + // add additional providers to the list if available + if (result_providers_future.get()->providers.size() > 0) + { + for (const auto &provider : result_providers_future.get()->providers) + { + providers_list.push_back(provider); + } + } } } + // if no semantic interfaces are availble for a given interface, add the interface instead else { interface_list.push_back(interface); + + // request providers of the semantic interface + request_providers->interface = interface; + request_providers->include_semantic = false; + + auto result_providers_future = get_providers_client_->async_send_request(request_providers); + + RCLCPP_INFO(this->get_logger(), "Requesting Providers information from Capabilities2 Server for semantic interface %s", interface.c_str()); + + // wait until data is received + if (rclcpp::spin_until_future_complete(shared_from_this(), result_providers_future) != rclcpp::FutureReturnCode::SUCCESS) + { + RCLCPP_INFO(this->get_logger(), "Failed to receive Providers information from Capabilities2 Server for %s", interface.c_str()); + return false; + } + + RCLCPP_INFO(this->get_logger(), "Received Providers information from Capabilities2 Server for %s", interface.c_str()); + + providers_list.push_back(result_providers_future.get()->default_provider); + + // add sematic interfaces to the list if available + if (result_providers_future.get()->providers.size() > 0) + { + for (const auto &provider : result_providers_future.get()->providers) + { + providers_list.push_back(provider); + } + } } } - // intialize a vector to accomodate elememnts from both + return true; + } + + /** + * @brief verify the plan using received interfaces + * + * @return `true` if interface retreival is successful,`false` otherwise + */ + bool verify_plan() + { + // intialize a vector to accomodate elements from both std::vector tag_list(interface_list.size() + control_tag_list.size()); std::merge(interface_list.begin(), interface_list.end(), control_tag_list.begin(), control_tag_list.end(), tag_list.begin()); + // verify whether document got 'plan' tags if (!capabilities2_xml_parser::check_plan_tag(document)) + { + RCLCPP_INFO(this->get_logger(), "Execution plan is not compatible. Please recheck and update"); return false; + } - tinyxml2::XMLElement* plan = capabilities2_xml_parser::get_plan(document); + // extract the components within the 'plan' tags + tinyxml2::XMLElement *plan = capabilities2_xml_parser::get_plan(document); - if (!capabilities2_xml_parser::check_tags(plan, interface_list, control_tag_list)) + // verify whether the plan is valid + if (!capabilities2_xml_parser::check_tags(plan, interface_list, providers_list, control_tag_list)) { RCLCPP_INFO(this->get_logger(), "Execution plan is faulty. Please recheck and update"); return false; } - + return true; } - void execute_plan() + /** + * @brief establish the bond with capabilities2 server + * + * @return `true` if bond establishing is successful,`false` otherwise + */ + bool establish_bond() + { + // create bond establishing server request + auto request_bond = std::make_shared(); + + // send the request + auto result_future = establish_bond_client_->async_send_request(request_bond); + + RCLCPP_INFO(this->get_logger(), "Requesting to establish bond with Capabilities2 Server"); + + // wait for the result + if (rclcpp::spin_until_future_complete(shared_from_this(), result_future) != rclcpp::FutureReturnCode::SUCCESS) + { + RCLCPP_INFO(this->get_logger(), "Failed to establish bond with Capabilities2 Server"); + return false; + } + + RCLCPP_INFO(this->get_logger(), "Established bond with Capabilities2 Server"); + + bond_id = result_future.get()->bond_id; + + return true; + } + + /** + * @brief execute the plan + * + * @param server_goal_handle goal handle of the server + */ + void execute_plan(const std::shared_ptr> server_goal_handle) { + auto result = std::make_shared(); + + // verify the plan + if (!request_information()) + { + RCLCPP_INFO(this->get_logger(), "Interface retreival failed"); + + // TODO: improve with error codes + result->success = false; + server_goal_handle->canceled(result); + + RCLCPP_INFO(this->get_logger(), "Server Execution Cancelled"); + } + + // verify the plan if (!verify_plan()) { RCLCPP_INFO(this->get_logger(), "Plan verification failed"); + + // TODO: improve with error codes + result->success = false; + server_goal_handle->canceled(result); + + RCLCPP_INFO(this->get_logger(), "Server Execution Cancelled"); } - // if (verify_plan()) - // { - // establish_bonds(); - - // use_capability(); - - // trigger_capability(); - // } - // else - // { - // request_updated_plan(); - // } - - // RCLCPP_INFO(this->get_logger(), "Executing goal"); - // rclcpp::Rate loop_rate(1); - // const auto goal = goal_handle->get_goal(); - // auto feedback = std::make_shared(); - // auto &sequence = feedback->partial_sequence; - // sequence.push_back(0); - // sequence.push_back(1); - // auto result = std::make_shared(); - - // for (int i = 1; (i < goal->order) && rclcpp::ok(); ++i) - // { - // // Check if there is a cancel request - // if (goal_handle->is_canceling()) - // { - // result->sequence = sequence; - // goal_handle->canceled(result); - // RCLCPP_INFO(this->get_logger(), "Goal canceled"); - // return; - // } - // // Update sequence - // sequence.push_back(sequence[i] + sequence[i - 1]); - // // Publish feedback - // goal_handle->publish_feedback(feedback); - // RCLCPP_INFO(this->get_logger(), "Publish feedback"); - - // loop_rate.sleep(); - // } - - // // Check if goal is done - // if (rclcpp::ok()) - // { - // result->sequence = sequence; - // goal_handle->succeed(result); - // RCLCPP_INFO(this->get_logger(), "Goal succeeded"); - // } + // extract the plan from the XMLDocument + tinyxml2::XMLElement *plan = capabilities2_xml_parser::get_plan(document); + + // Extract the connections from the plan + capabilities2_xml_parser::extract_connections(plan, connection_list); + + // estasblish the bond with the server + if (!establish_bond()) + { + RCLCPP_INFO(this->get_logger(), "Establishing bond failed"); + + // TODO: improve with error codes + result->success = false; + server_goal_handle->canceled(result); + + RCLCPP_INFO(this->get_logger(), "Server Execution Cancelled"); + } + + auto connection_goal_msg = capabilities2_msgs::action::Connections::Goal(); + + capabilities2_msgs::msg::CapabilityConnection connection_msg; + + for (const auto &connection : connection_list) + { + connection_msg.source.capability = connection.source_event; + connection_msg.source.provider = connection.source_provider; + connection_msg.target.capability = connection.target_event; + connection_msg.target.provider = connection.target_provider; + + if (connection.connection == capabilities2_executor::connection_type_t::ON_SUCCESS_START) + connection_msg.connection_type = capabilities2_msgs::msg::CapabilityConnection::ON_SUCCESS_START; + + if (connection.connection == capabilities2_executor::connection_type_t::ON_START_START) + connection_msg.connection_type = capabilities2_msgs::msg::CapabilityConnection::ON_START_START; + + if (connection.connection == capabilities2_executor::connection_type_t::ON_FAILURE_START) + connection_msg.connection_type = capabilities2_msgs::msg::CapabilityConnection::ON_FAILURE_START; + + connection_msg.target_parameters = capabilities2_xml_parser::convert_to_string(connection.event_element); + + connection_goal_msg.connections.push_back(connection_msg); + } + + auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); + + // send goal options + // goal response callback + send_goal_options.goal_response_callback = + [this, server_goal_handle](const rclcpp_action::ClientGoalHandle::SharedPtr &goal_handle) + { + if (!goal_handle) + { + RCLCPP_ERROR(this->get_logger(), "Goal was rejected by server"); + + auto result = std::make_shared(); + + // TODO: improve with error codes + result->success = false; + server_goal_handle->canceled(result); + + RCLCPP_INFO(this->get_logger(), "Server Execution Cancelled"); + } + else + { + RCLCPP_INFO(this->get_logger(), "Goal accepted by server, waiting for result"); + } + }; + + // result callback + send_goal_options.result_callback = + [this, server_goal_handle](const rclcpp_action::ClientGoalHandle::WrappedResult &result) + { + auto result_out = std::make_shared(); + + switch (result.code) + { + case rclcpp_action::ResultCode::SUCCEEDED: + break; + case rclcpp_action::ResultCode::ABORTED: + RCLCPP_ERROR(this->get_logger(), "Goal was aborted"); + + // TODO: improve with error codes + result_out->success = false; + server_goal_handle->canceled(result_out); + + RCLCPP_INFO(this->get_logger(), "Server Execution Cancelled"); + + return; + case rclcpp_action::ResultCode::CANCELED: + RCLCPP_ERROR(this->get_logger(), "Goal was canceled"); + + // TODO: improve with error codes + result_out->success = false; + server_goal_handle->canceled(result_out); + + RCLCPP_INFO(this->get_logger(), "Server Execution Cancelled"); + + return; + default: + RCLCPP_ERROR(this->get_logger(), "Unknown result code"); + + // TODO: improve with error codes + result_out->success = false; + server_goal_handle->canceled(result_out); + + RCLCPP_INFO(this->get_logger(), "Server Execution Cancelled"); + return; + } + + if (result.result->failed_connections.size() > 0) + { + // TODO: improve with error codes + result_out->success = false; + + for (const auto &failed_connection : result.result->failed_connections) + { + RCLCPP_ERROR(this->get_logger(), "Failed Events : %s", failed_connection.target_parameters.c_str()); + + result_out->failed_elements.push_back(failed_connection.target_parameters); + } + + server_goal_handle->canceled(result_out); + + RCLCPP_ERROR(this->get_logger(), "Server Execution Cancelled"); + } + else + { + // TODO: improve with error codes + result_out->success = true; + server_goal_handle->succeed(result_out); + + RCLCPP_INFO(this->get_logger(), "Server Execution Succeeded"); + } + + rclcpp::shutdown(); + }; + + this->client_capabilities_->async_send_goal(connection_goal_msg, send_goal_options); } +private: /** File Path link */ std::string plan_file_path; /** flag to select loading from file or accepting via action server */ bool read_file; + /** Bond ID between capabilities server and this client */ + std::string bond_id; + /** XML Document */ tinyxml2::XMLDocument document; + /** vector of connections */ + std::vector connection_list; + /** Execution Thread */ std::shared_ptr execution_thread; /** Interface List */ std::vector interface_list; + /** Providers List */ + std::vector providers_list; + /** Control flow List */ std::vector control_tag_list; - /** action server */ + /** action client for connecting with capabilities server*/ + rclcpp_action::Client::SharedPtr client_capabilities_; + + /** action server that exposes executor*/ std::shared_ptr> planner_server_; /** action server goal handle*/ @@ -264,6 +504,9 @@ class CapabilitiesExecutor : public rclcpp::Node /** Get semantic interfaces from capabilities server */ rclcpp::Client::SharedPtr get_sem_interf_client_; + /** Get providers from capabilities server */ + rclcpp::Client::SharedPtr get_providers_client_; + /** establish bond */ rclcpp::Client::SharedPtr establish_bond_client_; }; \ No newline at end of file diff --git a/capabilities2_executor/include/capabilities2_executor/capabilities_file_parser.hpp b/capabilities2_executor/include/capabilities2_executor/capabilities_file_parser.hpp new file mode 100644 index 0000000..a99bd4a --- /dev/null +++ b/capabilities2_executor/include/capabilities2_executor/capabilities_file_parser.hpp @@ -0,0 +1,135 @@ +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include + +/** + * @brief Capabilities Executor File Parser + * + * Capabilities Executor File Parser node that provides a ROS client for the capabilities executor. + * Will read an XML file that implements a plan and send it to the server + */ + +class CapabilitiesFileParser : public rclcpp::Node +{ +public: + CapabilitiesFileParser(const rclcpp::NodeOptions &options = rclcpp::NodeOptions()) + : Node("Capabilities2_File_Parser", options) + { + declare_parameter("plan_file_path", "plan.xml"); + plan_file_path = get_parameter("plan_file_path").as_string(); + + this->client_ptr_ = rclcpp_action::create_client(this, "~/capabilities"); + + this->timer_ = this->create_wall_timer(std::chrono::milliseconds(500), std::bind(&CapabilitiesFileParser::send_goal, this)); + } + + void send_goal() + { + using namespace std::placeholders; + + this->timer_->cancel(); + + // try to load the file + tinyxml2::XMLError xml_status = document.LoadFile(plan_file_path.c_str()); + + // check if the file loading failed + if (xml_status != tinyxml2::XMLError::XML_SUCCESS) + { + RCLCPP_INFO(this->get_logger(), "Loading the file from path : %s failed", plan_file_path.c_str()); + rclcpp::shutdown(); + } + + if (!this->client_ptr_->wait_for_action_server()) + { + RCLCPP_ERROR(this->get_logger(), "Action server not available after waiting"); + rclcpp::shutdown(); + } + + auto goal_msg = capabilities2_msgs::action::Plan::Goal(); + goal_msg.plan = capabilities2_xml_parser::convert_to_string(document); + + RCLCPP_INFO(this->get_logger(), "Sending goal"); + + auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); + + // send goal options + // goal response callback + send_goal_options.goal_response_callback = + [this](const rclcpp_action::ClientGoalHandle::SharedPtr &goal_handle) + { + if (!goal_handle) + { + RCLCPP_ERROR(this->get_logger(), "Goal was rejected by server"); + } + else + { + RCLCPP_INFO(this->get_logger(), "Goal accepted by server, waiting for result"); + } + }; + + // result callback + send_goal_options.result_callback = + [this](const rclcpp_action::ClientGoalHandle::WrappedResult &result) + { + switch (result.code) + { + case rclcpp_action::ResultCode::SUCCEEDED: + break; + case rclcpp_action::ResultCode::ABORTED: + RCLCPP_ERROR(this->get_logger(), "Goal was aborted"); + return; + case rclcpp_action::ResultCode::CANCELED: + RCLCPP_ERROR(this->get_logger(), "Goal was canceled"); + return; + default: + RCLCPP_ERROR(this->get_logger(), "Unknown result code"); + return; + } + + if (result.result->success) + { + RCLCPP_ERROR(this->get_logger(), "Plan executed successfully"); + } + else + { + RCLCPP_ERROR(this->get_logger(), "Plan failed to complete"); + + if (result.result->failed_elements.size() > 0) + { + RCLCPP_ERROR(this->get_logger(), "Plan failed due to incompatible XMLElements in the plan"); + + for (const auto &failed_element : result.result->failed_elements) + RCLCPP_ERROR(this->get_logger(), "Failed Elements : %s", failed_element.c_str()); + } + } + + rclcpp::shutdown(); + }; + + this->client_ptr_->async_send_goal(goal_msg, send_goal_options); + } + +private: + /** File Path link */ + std::string plan_file_path; + + /** XML Document */ + tinyxml2::XMLDocument document; + + /** action client */ + rclcpp_action::Client::SharedPtr client_ptr_; + + /** action server */ + rclcpp::TimerBase::SharedPtr timer_; +}; \ No newline at end of file diff --git a/capabilities2_executor/include/capabilities2_executor/capabilities_xml_parser.hpp b/capabilities2_executor/include/capabilities2_executor/capabilities_xml_parser.hpp index cf9571f..102d345 100644 --- a/capabilities2_executor/include/capabilities2_executor/capabilities_xml_parser.hpp +++ b/capabilities2_executor/include/capabilities2_executor/capabilities_xml_parser.hpp @@ -1,6 +1,7 @@ #include #include #include +#include namespace capabilities2_xml_parser { @@ -11,7 +12,7 @@ namespace capabilities2_xml_parser * * @return plan in the form of tinyxml2::XMLElement* */ - tinyxml2::XMLElement* get_plan(tinyxml2::XMLDocument &document) + tinyxml2::XMLElement *get_plan(tinyxml2::XMLDocument &document) { return document.FirstChildElement("Plan"); } @@ -58,40 +59,82 @@ namespace capabilities2_xml_parser 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 std::string containing valid event tags - * @param control_list std::string containing valid control tags + * @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 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 returnValue = true; + 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, control_list); + 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, control_list); + returnValue = returnValue and capabilities2_xml_parser::check_tags(element->NextSiblingElement(), events_list, providers_list, control_list); if (foundInControl and !hasSiblings) returnValue = returnValue; @@ -101,14 +144,17 @@ namespace capabilities2_xml_parser } else if (typetag == "Event") { - if (foundInEvents and hasSiblings) - returnValue = returnValue and capabilities2_xml_parser::check_tags(element->NextSiblingElement(), events_list, control_list); + 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 !hasSiblings) + if (foundInEvents and foundInProviders and !hasSiblings) returnValue = returnValue; if (!foundInEvents) return false; + + if (!foundInProviders) + return false; } else { @@ -118,6 +164,102 @@ namespace capabilities2_xml_parser return returnValue; } - + /** + * @brief Adds an event connection to the capabilities2 fabric + * + * @param source_event the source event from which the connection would start + * @param source_provider provider of the source event + * @param target_event the target event which will be connected via the connection + * @param target_provider provider of the target event + * @param connection the type of connection + * @param event_element the tinyxml2::XMLElement which contains runtime parameters + * + * @return The connection object + */ + capabilities2_executor::connection_t create_connection(std::string source_event, std::string source_provider, + std::string target_event, std::string target_provider, + capabilities2_executor::connection_type_t connection, tinyxml2::XMLElement *event_element) + { + capabilities2_executor::connection_t connect; + + connect.source_event = source_event; + connect.source_provider = source_provider; + connect.target_event = target_event; + connect.target_provider = target_provider; + connect.connection = connection; + connect.event_element = event_element; + + return connect; + } + + /** + * @brief parse through the plan and extract the connections + * + * @param element XML Element to be evaluated + * @param connection_list std::vector containing extracted connections + * @param connection the type of connection + * @param source_event source event name. if not provided, "start" is taken as default + * @param source_provider provider of the source event, if not provided "" is taken as default + * @param target_event target event name. if not provided, "stop" is taken as default + * @param target_provider provider of the target event. if not provided, "stop" is taken as default + */ + void extract_connections(tinyxml2::XMLElement *element, std::vector &connection_list, + capabilities2_executor::connection_type_t connection = capabilities2_executor::connection_type_t::ON_SUCCESS_START, + std::string source_event = "start", std::string source_provider = "", + std::string target_event = "stop", std::string target_provider = "") + { + const char **name; + const char **provider; + + element->QueryStringAttribute("name", name); + element->QueryStringAttribute("provider", provider); + + std::string typetag(element->Name()); + std::string nametag(*name); + std::string providertag(*provider); + + bool hasChildren = !element->NoChildren(); + bool hasSiblings = !capabilities2_xml_parser::isLastElement(element); + + if ((typetag == "Control") and (nametag == "sequential")) + { + if (hasChildren) + capabilities2_xml_parser::extract_connections(element->FirstChildElement(), connection_list, capabilities2_executor::connection_type_t::ON_SUCCESS_START, + source_event, source_provider, target_event, target_provider); + + if (hasSiblings) + capabilities2_xml_parser::extract_connections(element->NextSiblingElement(), connection_list, connection, + source_event, source_provider, target_event, target_provider); + } + else if ((typetag == "Control") and (nametag == "parallel")) + { + if (hasChildren) + capabilities2_xml_parser::extract_connections(element->FirstChildElement(), connection_list, capabilities2_executor::connection_type_t::ON_START_START, + source_event, source_provider, target_event, target_provider); + + if (hasSiblings) + capabilities2_xml_parser::extract_connections(element->NextSiblingElement(), connection_list, connection, + source_event, source_provider, target_event, target_provider); + } + else if ((typetag == "Control") and (nametag == "recovery")) + { + if (hasChildren) + capabilities2_xml_parser::extract_connections(element->FirstChildElement(), connection_list, capabilities2_executor::connection_type_t::ON_FAILURE_START, + source_event, source_provider, target_event, target_provider); + + if (hasSiblings) + capabilities2_xml_parser::extract_connections(element->NextSiblingElement(), connection_list, connection, + source_event, source_provider, target_event, target_provider); + } + else if (typetag == "Event") + { + capabilities2_executor::connection_t connection_obj = create_connection(source_event, source_provider, nametag, providertag, connection, element); + connection_list.push_back(connection_obj); + + if (hasSiblings) + capabilities2_xml_parser::extract_connections(element->NextSiblingElement(), connection_list, connection, + nametag, providertag, target_event, target_provider); + } + } } // namespace capabilities2_xml_parser diff --git a/capabilities2_executor/include/capabilities2_executor/structs/connection.hpp b/capabilities2_executor/include/capabilities2_executor/structs/connection.hpp new file mode 100644 index 0000000..30b8a21 --- /dev/null +++ b/capabilities2_executor/include/capabilities2_executor/structs/connection.hpp @@ -0,0 +1,27 @@ +#include +#include + +namespace capabilities2_executor +{ + enum connection_type_t + { + ON_START_START, + ON_START_STOP, + ON_SUCCESS_START, + ON_SUCCESS_STOP, + ON_FAILURE_START, + ON_FAILURE_STOP, + ON_TERMINATE_START, + ON_TERMINATE_STOP + }; + + struct connection_t { + std::string source_event; + std::string source_provider; + std::string target_event; + std::string target_provider; + connection_type_t connection; + tinyxml2::XMLElement* event_element; + }; + +} // namespace capabilities2_executor diff --git a/capabilities2_executor/src/capabilities_file_parser.cpp b/capabilities2_executor/src/capabilities_file_parser.cpp new file mode 100644 index 0000000..00c2410 --- /dev/null +++ b/capabilities2_executor/src/capabilities_file_parser.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_msgs/CMakeLists.txt b/capabilities2_msgs/CMakeLists.txt index c61dc79..669eb9c 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/CapabilityConnection.msg" ) set(srv_files @@ -46,6 +47,7 @@ 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 new file mode 100644 index 0000000..d72b242 --- /dev/null +++ b/capabilities2_msgs/action/Connections.action @@ -0,0 +1,13 @@ +# action to send a list of interface connections to capabilities server +# goal +std_msgs/Header header +CapabilityConnection[] connections +--- +# result +std_msgs/Header header +CapabilityConnection[] failed_connections +--- +# feedback +std_msgs/Header header +int32 success_count +int32 total_count diff --git a/capabilities2_msgs/action/Plan.action b/capabilities2_msgs/action/Plan.action index c36f453..ad84fa9 100644 --- a/capabilities2_msgs/action/Plan.action +++ b/capabilities2_msgs/action/Plan.action @@ -6,6 +6,7 @@ string plan # result std_msgs/Header header bool success +string[] failed_elements --- # feedback std_msgs/Header header diff --git a/capabilities2_msgs/msg/CapabilityConnection.msg b/capabilities2_msgs/msg/CapabilityConnection.msg new file mode 100644 index 0000000..04b538b --- /dev/null +++ b/capabilities2_msgs/msg/CapabilityConnection.msg @@ -0,0 +1,14 @@ +# 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 diff --git a/capabilities2_runner/include/capabilities2_runner/action_runner.hpp b/capabilities2_runner/include/capabilities2_runner/action_runner.hpp index efc9ef9..fc703f8 100644 --- a/capabilities2_runner/include/capabilities2_runner/action_runner.hpp +++ b/capabilities2_runner/include/capabilities2_runner/action_runner.hpp @@ -185,14 +185,14 @@ class ActionRunner : public RunnerBase throw runner_exception("no action resources found for interface: " + run_config_.interface); } - /**< action client */ + /** action client */ std::shared_ptr> action_client_; - /**< Send Goal Option struct to link result_callback, feedback_callback and goal_response_callback with action client + /** Send Goal Option struct to link result_callback, feedback_callback and goal_response_callback with action client */ typename rclcpp_action::Client::SendGoalOptions send_goal_options_; - /**< goal handle parameter to capture goal response from goal_response_callback */ + /** goal handle parameter to capture goal response from goal_response_callback */ typename rclcpp_action::ClientGoalHandle::SharedPtr goal_handle_; }; From 52d244ade3170119d585babf4e9409cdb0841db0 Mon Sep 17 00:00:00 2001 From: mik-p Date: Thu, 3 Oct 2024 12:34:35 +0000 Subject: [PATCH 2/2] small fixes --- TODO.md | 1 + .../include/capabilities2_server/capabilities_api.hpp | 10 ++++++++++ .../include/capabilities2_server/models/provider.hpp | 2 +- .../include/capabilities2_server/runner_cache.hpp | 9 +++++++++ 4 files changed, 21 insertions(+), 1 deletion(-) diff --git a/TODO.md b/TODO.md index 844ef13..0a7e405 100644 --- a/TODO.md +++ b/TODO.md @@ -14,3 +14,4 @@ - [ ] 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_server/include/capabilities2_server/capabilities_api.hpp b/capabilities2_server/include/capabilities2_server/capabilities_api.hpp index 832f9b3..d725b8c 100644 --- a/capabilities2_server/include/capabilities2_server/capabilities_api.hpp +++ b/capabilities2_server/include/capabilities2_server/capabilities_api.hpp @@ -152,8 +152,17 @@ class CapabilitiesAPI */ void stop_capability(const std::string& capability) { + // make sure provider exists + // 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()); + return; + } + // get the provider from runner std::string provider = runner_cache_.provider(capability); + // get the running model from the db models::running_model_t running = cap_db_->get_running(provider); @@ -182,6 +191,7 @@ class CapabilitiesAPI catch (const capabilities2_runner::runner_exception& e) { RCLCPP_WARN(node_logging_interface_ptr_->get_logger(), "could not stop runner: %s", e.what()); + return; } // log diff --git a/capabilities2_server/include/capabilities2_server/models/provider.hpp b/capabilities2_server/include/capabilities2_server/models/provider.hpp index 1032e8d..1c20da6 100644 --- a/capabilities2_server/include/capabilities2_server/models/provider.hpp +++ b/capabilities2_server/include/capabilities2_server/models/provider.hpp @@ -76,7 +76,7 @@ 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 + "'" + (defined() ? ", '" + definition_str + "'" : ""); + YAML::Dump(remappings.to_yaml()) + "', '" + runner + "', '" + definition_str + "'"; } }; diff --git a/capabilities2_server/include/capabilities2_server/runner_cache.hpp b/capabilities2_server/include/capabilities2_server/runner_cache.hpp index da4f07a..e922dea 100644 --- a/capabilities2_server/include/capabilities2_server/runner_cache.hpp +++ b/capabilities2_server/include/capabilities2_server/runner_cache.hpp @@ -152,6 +152,9 @@ class RunnerCache */ const std::string provider(const std::string& capability) { + if (!running(capability)) + throw capabilities2_runner::runner_exception("capability runner not found: " + capability); + return runner_cache_[capability]->get_provider(); } @@ -163,6 +166,9 @@ class RunnerCache */ const std::string started_by(const std::string& capability) { + if (!running(capability)) + throw capabilities2_runner::runner_exception("capability runner not found: " + capability); + return runner_cache_[capability]->get_started_by(); } @@ -174,6 +180,9 @@ class RunnerCache */ const std::string pid(const std::string& capability) { + if (!running(capability)) + throw capabilities2_runner::runner_exception("capability runner not found: " + capability); + return runner_cache_[capability]->get_pid(); }