From 4f759e467c908cc34a43587bf752004875fb5126 Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Sun, 26 Jan 2025 17:47:06 +1100 Subject: [PATCH 1/6] 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 2/6] 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 3/6] 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 4/6] 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 5/6] 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 6/6] 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