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 9faebc0..3837283 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(rclcpp_components REQUIRED) # Locate the static version of tinyxml2 find_library(TINYXML2_LIB NAMES tinyxml2 PATHS /usr/local/lib NO_DEFAULT_PATH) @@ -29,8 +30,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} @@ -48,24 +53,100 @@ install(TARGETS ${PROJECT_NAME} DESTINATION lib/${PROJECT_NAME} ) -add_executable(capabilities2_file_parser - src/capabilities_file_parser.cpp +############################################################################ +# 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 +) + +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/capabilities_fabric_client_node.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} ) +############################################################################ +# 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 +) + +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/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..e0b5f06 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 @@ -54,17 +55,21 @@ 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 = capabilities2_xml_parser::get_control_list(); + control_tag_list = xml_parser::get_control_list(); } /** - * @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,11 @@ 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; + bond_client_->stop(); + return rclcpp_action::CancelResponse::ACCEPT; } @@ -143,15 +151,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 +175,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 +201,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 +245,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 +256,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 +300,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 +318,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 +330,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) { @@ -363,36 +377,35 @@ class CapabilitiesFabric : public rclcpp::Node for (const auto& rejected_element : rejected_list) { - RCLCPP_ERROR(this->get_logger(), "Failed Events : %s", rejected_element.c_str()); result->failed_elements.push_back(rejected_element); } + goal_handle_->abort(result); - goal_handle->abort(result); - RCLCPP_ERROR(this->get_logger(), "Server Execution Cancelled"); + process_feedback(result->message); } 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 = capabilities2_xml_parser::get_plan(document); + 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 - capabilities2_xml_parser::extract_connections(plan, connection_map); + 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 +413,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(); @@ -410,27 +423,27 @@ 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); + 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 = capabilities2_xml_parser::get_plan(document); + 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 (!capabilities2_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 +451,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 +470,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 +490,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 +500,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 +530,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 +556,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 (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; - - RCLCPP_INFO(this->get_logger(), "Source capability : %s from provider : %s", request_configure->source.capability.c_str(), - request_configure->source.provider.c_str()); } else { @@ -591,14 +600,11 @@ class CapabilitiesFabric : public rclcpp::Node request_configure->source.provider = ""; } - if (capabilities2_xml_parser::convert_to_string(capabilities[completed_configurations_].target_on_start.parameters, - request_configure->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; 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 { @@ -606,14 +612,11 @@ 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, - request_configure->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; 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 { @@ -621,14 +624,11 @@ 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, - request_configure->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; 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 { @@ -636,14 +636,11 @@ 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, - request_configure->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; 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 { @@ -654,12 +651,12 @@ 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, goal_handle, 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"; - print_and_result(goal_handle, status, false); + process_result(status); return; } @@ -669,18 +666,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,32 +685,31 @@ 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(); 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; // send the request - auto result_future = - trig_capability_client_->async_send_request(request_trigger, [this, goal_handle](TriggerCapabilityClient::SharedFuture future) { - if (!future.valid()) - { - status = "Failed to trigger capability " + connection_map[0].source.runner; - print_and_result(goal_handle, status, false); - 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; - print_and_feedback(goal_handle, status, false); + status = "Successfully triggered capability " + connection_map[0].source.runner; + process_feedback(status); - print_and_result(goal_handle, "Successfully launched capabilities2 fabric", true); - }); + process_result("Successfully launched capabilities2 fabric", true); + }); } void check_service(bool wait_for_logic, const std::string& service_name) @@ -721,40 +717,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,14 +789,17 @@ 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; /** vector of connections */ - std::map connection_map; + std::map connection_map; /** Interface List */ std::vector is_semantic_list; @@ -817,7 +826,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/capabilities_fabric_client.hpp b/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric_client.hpp new file mode 100644 index 0000000..a13976c --- /dev/null +++ b/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric_client.hpp @@ -0,0 +1,270 @@ +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include + +#include + +#include +#include +#include + +/** + * @brief Capabilities Executor File Parser + * + * Capabilities Executor File Parser node that provides a ROS client for the capabilities executor. + * Will read an XML file that implements a plan and send it to the server + */ + +class CapabilitiesFabricClient : public rclcpp::Node +{ +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(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(); + } + + /** + * @brief Initializer function + * + */ + 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->planner_client_ = rclcpp_action::create_client(shared_from_this(), "/capabilities_fabric"); + + if (!this->planner_client_->wait_for_action_server(std::chrono::seconds(5))) + { + status_->error("Action server not available after waiting"); + rclcpp::shutdown(); + return; + } + + status_->info("Sucessfully connected to the capabilities_fabric action server"); + + // try to load the file + tinyxml2::XMLError xml_status = document.LoadFile(plan_file_path.c_str()); + + // check if the file loading failed + if (xml_status != tinyxml2::XMLError::XML_SUCCESS) + { + status = "Error loading plan: " + plan_file_path + ", Error: " + document.ErrorName(); + status_->error(status); + rclcpp::shutdown(); + } + + status = "Plan loaded from : " + plan_file_path; + status_->info(status); + + send_goal(document); + } + +private: + void send_goal(tinyxml2::XMLDocument& document_xml) + { + auto goal_msg = Plan::Goal(); + + xml_parser::convert_to_string(document_xml, goal_msg.plan); + + status = "Following plan was loaded :\n\n " + goal_msg.plan; + status_->info(status); + + status_->info("Sending goal to the capabilities_fabric action server"); + + auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); + + // send goal options + // goal response callback + send_goal_options.goal_response_callback = [this](const GoalHandlePlan::SharedPtr& goal_handle) { + if (!goal_handle) + { + 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 GoalHandlePlan::WrappedResult& result) { + switch (result.code) + { + case rclcpp_action::ResultCode::SUCCEEDED: + fabric_state = fabric_status::SUCCEEDED; + break; + case rclcpp_action::ResultCode::ABORTED: + status_->error("Goal was aborted"); + fabric_state = fabric_status::ABORTED; + break; + case rclcpp_action::ResultCode::CANCELED: + status_->error("Goal was canceled"); + fabric_state = fabric_status::CANCELED; + break; + default: + status_->error("Unknown result code"); + fabric_state = fabric_status::FAILED; + break; + } + + if (result.result->success) + { + status_->info("Plan executed successfully"); + } + else + { + status_->error("Plan failed to complete"); + + if (result.result->failed_elements.size() > 0) + { + status_->error("Plan failed due to incompatible XMLElements in the plan"); + + for (const auto& failed_element : result.result->failed_elements) + status_->error_element(failed_element); + } + } + }; + + 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 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; +}; diff --git a/capabilities2_fabric/include/capabilities2_fabric/capabilities_file_parser.hpp b/capabilities2_fabric/include/capabilities2_fabric/capabilities_file_parser.hpp deleted file mode 100644 index d7614e9..0000000 --- a/capabilities2_fabric/include/capabilities2_fabric/capabilities_file_parser.hpp +++ /dev/null @@ -1,138 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include - -#include -#include - -/** - * @brief Capabilities Executor File Parser - * - * Capabilities Executor File Parser node that provides a ROS client for the capabilities executor. - * Will read an XML file that implements a plan and send it to the server - */ - -class CapabilitiesFileParser : public rclcpp::Node -{ -public: - CapabilitiesFileParser() : Node("Capabilities2_File_Parser") - { - declare_parameter("plan_file_path", "install/capabilities2_fabric/share/capabilities2_fabric/plans/default.xml"); - plan_file_path = get_parameter("plan_file_path").as_string(); - } - - /** - * @brief Initializer function - * - */ - void initialize() - { - // Create the action client for capabilities_fabric after the node is fully constructed - this->client_ptr_ = rclcpp_action::create_client(shared_from_this(), "/capabilities_fabric"); - - if (!this->client_ptr_->wait_for_action_server(std::chrono::seconds(5))) - { - RCLCPP_ERROR(this->get_logger(), "Action server not available after waiting"); - rclcpp::shutdown(); - return; - } - - RCLCPP_INFO(this->get_logger(), "Sucessfully connected to the capabilities_fabric action server"); - - send_goal(); - } - - void send_goal() - { - // 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()); - rclcpp::shutdown(); - } - - RCLCPP_INFO(this->get_logger(), "Plan loaded from : %s", plan_file_path.c_str()); - - auto goal_msg = capabilities2_msgs::action::Plan::Goal(); - - capabilities2_xml_parser::convert_to_string(document, goal_msg.plan); - - RCLCPP_INFO(this->get_logger(), "Following plan was loaded :\n\n%s ", goal_msg.plan.c_str()); - - RCLCPP_INFO(this->get_logger(), "Sending goal to the capabilities_fabric action server"); - - auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); - - // send goal options - // goal response callback - send_goal_options.goal_response_callback = - [this](const rclcpp_action::ClientGoalHandle::SharedPtr& goal_handle) { - if (!goal_handle) - { - RCLCPP_ERROR(this->get_logger(), "Goal was rejected by server"); - } - else - { - RCLCPP_INFO(this->get_logger(), "Goal accepted by server, waiting for result"); - } - }; - - // result callback - send_goal_options.result_callback = [this](const rclcpp_action::ClientGoalHandle::WrappedResult& result) { - switch (result.code) - { - case rclcpp_action::ResultCode::SUCCEEDED: - break; - case rclcpp_action::ResultCode::ABORTED: - RCLCPP_ERROR(this->get_logger(), "Goal was aborted"); - return; - case rclcpp_action::ResultCode::CANCELED: - RCLCPP_ERROR(this->get_logger(), "Goal was canceled"); - return; - default: - RCLCPP_ERROR(this->get_logger(), "Unknown result code"); - return; - } - - if (result.result->success) - { - RCLCPP_INFO(this->get_logger(), "Plan executed successfully"); - } - else - { - RCLCPP_ERROR(this->get_logger(), "Plan failed to complete"); - - if (result.result->failed_elements.size() > 0) - { - RCLCPP_ERROR(this->get_logger(), "Plan failed due to incompatible XMLElements in the plan"); - - for (const auto& failed_element : result.result->failed_elements) - RCLCPP_ERROR(this->get_logger(), "Failed Elements : %s", failed_element.c_str()); - } - } - }; - - this->client_ptr_->async_send_goal(goal_msg, send_goal_options); - } - -private: - /** File Path link */ - std::string plan_file_path; - - /** XML Document */ - tinyxml2::XMLDocument document; - - /** action client */ - rclcpp_action::Client::SharedPtr client_ptr_; -}; \ No newline at end of file diff --git a/capabilities2_fabric/include/capabilities2_fabric/capabilities_xml_parser.hpp b/capabilities2_fabric/include/capabilities2_fabric/capabilities_xml_parser.hpp deleted file mode 100644 index 6910d60..0000000 --- a/capabilities2_fabric/include/capabilities2_fabric/capabilities_xml_parser.hpp +++ /dev/null @@ -1,292 +0,0 @@ -#include -#include -#include -#include -#include - -namespace capabilities2_xml_parser -{ -/** - * @brief extract elements related plan - * - * @param document XML document to extract plan from - * - * @return plan in the form of tinyxml2::XMLElement* - */ -tinyxml2::XMLElement* get_plan(tinyxml2::XMLDocument& document) -{ - return document.FirstChildElement("Plan")->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 = !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 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 &= capabilities2_xml_parser::check_tags(logger, element->FirstChildElement(), events, providers, control, rejected); - - if (hasSiblings) - returnValue &= capabilities2_xml_parser::check_tags(logger, element->NextSiblingElement(), events, providers, control, rejected); - } - else if (typetag == "Event") - { - if (!foundInEvents || !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 &= capabilities2_xml_parser::check_tags(logger, element->NextSiblingElement(), events, providers, control, rejected); - } - else - { - RCLCPP_ERROR(logger, "xml element is not valid", parameter_string.c_str()); - rejected.push_back(parameter_string); - return false; - } - - return returnValue; -} - -/** - * @brief Returns control xml tags supported in extract_connections method - * - */ -std::vector get_control_list() -{ - std::vector tag_list; - - tag_list.push_back("sequential"); - tag_list.push_back("parallel"); - tag_list.push_back("recovery"); - - return tag_list; -} - -/** - * @brief parse through the plan and extract the connections - * - * @param element XML Element to be evaluated - * @param connections std::map containing extracted connections - * @param connection_id numerical id of the connection - * @param connection_type the type of connection - */ -void extract_connections(tinyxml2::XMLElement* element, std::map& connections, int connection_id = 0, - capabilities2_executor::connection_type_t connection_type = capabilities2_executor::connection_type_t::ON_SUCCESS) -{ - int predecessor_id; - - const char* name; - const char* provider; - - element->QueryStringAttribute("name", &name); - element->QueryStringAttribute("provider", &provider); - - std::string typetag(element->Name()); - - std::string nametag; - std::string providertag; - - if (name) - nametag = name; - else - nametag = ""; - - if (provider) - providertag = provider; - else - providertag = ""; - - bool hasChildren = !element->NoChildren(); - bool hasSiblings = !capabilities2_xml_parser::isLastElement(element); - - if ((typetag == "Control") and (nametag == "sequential")) - { - if (hasChildren) - capabilities2_xml_parser::extract_connections(element->FirstChildElement(), connections, connection_id, - capabilities2_executor::connection_type_t::ON_SUCCESS); - - if (hasSiblings) - capabilities2_xml_parser::extract_connections(element->NextSiblingElement(), connections, connection_id, connection_type); - } - else if ((typetag == "Control") and (nametag == "parallel")) - { - if (hasChildren) - capabilities2_xml_parser::extract_connections(element->FirstChildElement(), connections, connection_id, - capabilities2_executor::connection_type_t::ON_START); - - if (hasSiblings) - capabilities2_xml_parser::extract_connections(element->NextSiblingElement(), connections, connection_id, connection_type); - } - else if ((typetag == "Control") and (nametag == "recovery")) - { - if (hasChildren) - capabilities2_xml_parser::extract_connections(element->FirstChildElement(), connections, connection_id, - capabilities2_executor::connection_type_t::ON_FAILURE); - - if (hasSiblings) - capabilities2_xml_parser::extract_connections(element->NextSiblingElement(), connections, connection_id, connection_type); - } - else if (typetag == "Event") - { - capabilities2_executor::node_t node; - - node.source.runner = nametag; - node.source.provider = providertag; - node.source.parameters = element; - - predecessor_id = connection_id - 1; - - while (connections.count(connection_id) > 0) - connection_id += 1; - - connections[connection_id] = node; - - if ((connection_type == capabilities2_executor::connection_type_t::ON_SUCCESS) and (connection_id != 0)) - connections[predecessor_id].target_on_success = connections[connection_id].source; - - else if ((connection_type == capabilities2_executor::connection_type_t::ON_START) and (connection_id != 0)) - connections[predecessor_id].target_on_start = connections[connection_id].source; - - else if ((connection_type == capabilities2_executor::connection_type_t::ON_FAILURE) and (connection_id != 0)) - connections[predecessor_id].target_on_failure = connections[connection_id].source; - - if (hasSiblings) - capabilities2_xml_parser::extract_connections(element->NextSiblingElement(), connections, connection_id + 1, connection_type); - } -} - -} // namespace capabilities2_xml_parser diff --git a/capabilities2_fabric/include/capabilities2_fabric/utils/bond_client.hpp b/capabilities2_fabric/include/capabilities2_fabric/utils/bond_client.hpp index b1f286a..401391e 100644 --- a/capabilities2_fabric/include/capabilities2_fabric/utils/bond_client.hpp +++ b/capabilities2_fabric/include/capabilities2_fabric/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_fabric/include/capabilities2_fabric/utils/connection.hpp index d6ecc65..8aa94f4 100644 --- a/capabilities2_fabric/include/capabilities2_fabric/utils/connection.hpp +++ b/capabilities2_fabric/include/capabilities2_fabric/utils/connection.hpp @@ -1,7 +1,8 @@ +#pragma once #include #include -namespace capabilities2_executor +namespace capabilities2 { enum connection_type_t { @@ -26,4 +27,4 @@ namespace capabilities2_executor connection_t target_on_failure; }; -} // namespace capabilities2_executor +} // namespace capabilities2 diff --git a/capabilities2_fabric/include/capabilities2_fabric/utils/status_client.hpp b/capabilities2_fabric/include/capabilities2_fabric/utils/status_client.hpp new file mode 100644 index 0000000..c634155 --- /dev/null +++ b/capabilities2_fabric/include/capabilities2_fabric/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); + } + + /**error_element + * @brief publishes status information to the given topic and prints to the logging as info + * + * @param text Text to be published + * @param newline whether to add new line to the ROS_INFO print + */ + void info(const std::string &text, bool newline = false) + { + auto status_msg = Status(); + status_msg.header.stamp = node_->now(); + status_msg.origin_node = node_name_; + status_msg.text = text; + status_msg.failure = false; + + if (newline) + RCLCPP_INFO(node_->get_logger(), ""); + + RCLCPP_INFO(node_->get_logger(), status_msg.text.c_str()); + + status_publisher_->publish(status_msg); + } + + /** + * @brief publishes status information to the given topic and prints to the logging as error + * + * @param text Text to be published + * @param newline whether to add new line to the ROS_INFO print + */ + void error(const std::string &text, bool newline = false) + { + auto status_msg = Status(); + status_msg.header.stamp = node_->now(); + status_msg.origin_node = node_name_; + status_msg.text = text; + status_msg.failure = true; + status_msg.is_failed_element = false; + + if (newline) + RCLCPP_ERROR(node_->get_logger(), ""); + + RCLCPP_ERROR(node_->get_logger(), status_msg.text.c_str()); + + status_publisher_->publish(status_msg); + } + + /** + * @brief publishes status information to the given topic and prints to the logging as error + * + * @param text Text to be published + * @param newline whether to add new line to the ROS_INFO print + */ + void error_element(const std::string &element) + { + auto status_msg = Status(); + status_msg.header.stamp = node_->now(); + status_msg.origin_node = node_name_; + status_msg.text = "Failed element"; + status_msg.failure = true; + status_msg.is_failed_element = true; + status_msg.failed_element = element; + + std::string msg = status_msg.text + " : " + status_msg.failed_element; + + RCLCPP_ERROR(node_->get_logger(), msg.c_str()); + + status_publisher_->publish(status_msg); + } + +protected: + /** + * @brief Node pointer to access logging interface + * + */ + rclcpp::Node::SharedPtr node_; + + /** + * @brief publisher to publish execution status + * + */ + rclcpp::Publisher::SharedPtr status_publisher_; + + /** + * @brief Node name + * + */ + std::string node_name_; +}; \ No newline at end of file diff --git a/capabilities2_fabric/include/capabilities2_fabric/utils/xml_parser.hpp b/capabilities2_fabric/include/capabilities2_fabric/utils/xml_parser.hpp new file mode 100644 index 0000000..71f450d --- /dev/null +++ b/capabilities2_fabric/include/capabilities2_fabric/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_fabric/launch/fabric.launch.py b/capabilities2_fabric/launch/fabric.launch.py index 2320523..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='capabilities2_file_parser', - name='capabilities2_file_parser', - parameters=[executor_config], + executable='fabric_client', + name='fabric_client', + parameters=[fabric_config], output='screen' ) - # return return LaunchDescription([ - executor, - executor_file + capabilities2_fabric, + fabric_client ]) 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/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_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/capabilities_file_parser.cpp b/capabilities2_fabric/src/capabilities_fabric_client_node.cpp similarity index 60% rename from capabilities2_fabric/src/capabilities_file_parser.cpp rename to capabilities2_fabric/src/capabilities_fabric_client_node.cpp index bb08cf0..9bec80e 100644 --- a/capabilities2_fabric/src/capabilities_file_parser.cpp +++ b/capabilities2_fabric/src/capabilities_fabric_client_node.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 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_msgs/CMakeLists.txt b/capabilities2_msgs/CMakeLists.txt index 58edbe3..b50c9eb 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,9 @@ set(srv_files "srv/ConfigureCapability.srv" "srv/TriggerCapability.srv" "srv/Launch.srv" + "srv/GetFabricStatus.srv" + "srv/SetFabricPlan.srv" + "srv/CancelFabricPlan.srv" ) set(action_files diff --git a/capabilities2_msgs/msg/CapabilityEvent.msg b/capabilities2_msgs/msg/CapabilityEvent.msg index 1a52c94..581a67d 100644 --- a/capabilities2_msgs/msg/CapabilityEvent.msg +++ b/capabilities2_msgs/msg/CapabilityEvent.msg @@ -1,16 +1,33 @@ 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 FAILED=3 +uint8 SUCCEEDED=4 +uint8 UNDEFINED=5 + # Event type -string type +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_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/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 new file mode 100644 index 0000000..6e12c28 --- /dev/null +++ b/capabilities2_msgs/srv/GetFabricStatus.srv @@ -0,0 +1,11 @@ +std_msgs/Header header +--- +std_msgs/Header header +uint8 status +uint8 FABRIC_IDLE=0 +uint8 FABRIC_RUNNING=1 +uint8 FABRIC_CANCELED=2 +uint8 FABRIC_ABORTED=3 +uint8 FABRIC_FAILED=4 +uint8 FABRIC_SUCCEEDED=5 +uint8 UNKNOWN=6 \ 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_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/CMakeLists.txt b/capabilities2_server/CMakeLists.txt index 3eb130c..aa21c61 100644 --- a/capabilities2_server/CMakeLists.txt +++ b/capabilities2_server/CMakeLists.txt @@ -44,7 +44,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 @@ -84,7 +86,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 @@ -126,6 +130,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 diff --git a/capabilities2_server/include/capabilities2_server/capabilities_api.hpp b/capabilities2_server/include/capabilities2_server/capabilities_api.hpp index 55acc3b..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 @@ -46,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. * @@ -99,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); @@ -120,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; } } @@ -152,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()); } } @@ -167,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; } @@ -181,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); @@ -201,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); } /** @@ -225,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); } } @@ -273,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()); } } @@ -302,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; } @@ -314,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; } @@ -322,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; } @@ -331,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; } @@ -343,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; } @@ -351,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; } @@ -360,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; } @@ -372,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 @@ -613,104 +597,15 @@ class CapabilitiesAPI return running_capabilities; } - // 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() + /** + * @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::SERVER_READY; - - return event; + return atoi(runner_cache_.pid(capability).c_str()); } // bond api @@ -732,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); @@ -809,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_db.hpp b/capabilities2_server/include/capabilities2_server/capabilities_db.hpp index 2a724dd..d48e7ec 100644 --- a/capabilities2_server/include/capabilities2_server/capabilities_db.hpp +++ b/capabilities2_server/include/capabilities2_server/capabilities_db.hpp @@ -13,7 +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..1d4187e 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,10 +89,11 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI if (rebuild) { // remove db file if it exists - RCLCPP_INFO(get_logger(), "Rebuilding capabilities database"); + 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); } } @@ -97,12 +101,20 @@ 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 - connect(db_file, get_node_logging_interface()); - + event_publish("Connecting API with Database", false); + + 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); // load capabilities from package paths for (const auto& package_path : package_paths) @@ -110,11 +122,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 +194,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 @@ -246,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; } @@ -269,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; } @@ -311,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; } @@ -361,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); @@ -409,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; } @@ -457,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; } @@ -495,13 +500,15 @@ 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()); + 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); } } }; @@ -538,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; } @@ -576,13 +583,15 @@ 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()); + 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); } } }; @@ -648,6 +657,61 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI spec_file_file.close(); } + void event_publish(const std::string& text, bool is_server_ready = true, bool is_error = false) + { + auto message = capabilities2_msgs::msg::CapabilityEvent(); + + message.header.stamp = rclcpp::Clock().now(); + message.source.capability = ""; + message.source.provider = ""; + message.target.capability = ""; + message.target.provider = ""; + message.thread_id = 0; + message.text = text; + message.error = is_error; + message.pid = -1; + message.server_ready = is_server_ready; + + event_pub_->publish(message); + + if (is_error) + RCLCPP_ERROR(get_logger(), text.c_str()); + else + RCLCPP_INFO(get_logger(), text.c_str()); + } + + void event_publish_runner(capabilities2_msgs::msg::CapabilityEvent& message) + { + message.pid = get_pid(message.source.capability); + message.server_ready = true; + + event_pub_->publish(message); + + std::string text; + if (message.thread_id >= 0 and message.target.capability == "") + { + text = "[" + message.source.capability + "/" + std::to_string(message.thread_id) + "] " + message.text; + } + else if (message.thread_id < 0 and message.target.capability == "") + { + text = "[" + message.source.capability + "] " + message.text; + } + else if (message.thread_id >= 0 and message.target.capability != "") + { + text = "[" + message.source.capability + "/" + std::to_string(message.thread_id) + "] triggering " + + message.target.capability + " " + message.text; + } + else if (message.thread_id < 0 and message.target.capability != "") + { + text = "[" + message.source.capability + "] triggering " + message.target.capability + " " + message.text; + } + + if (message.error) + RCLCPP_ERROR(get_logger(), text.c_str()); + else + RCLCPP_INFO(get_logger(), text.c_str()); + } + private: // loop hz double loop_hz_; diff --git a/capabilities2_server/include/capabilities2_server/models/header.hpp b/capabilities2_server/include/capabilities2_server/models/header.hpp index 3451957..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 69fa8ab..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 91269fa..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 b479aea..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_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 diff --git a/capabilities2_server/include/capabilities2_server/sanitizer/sql_safe.hpp b/capabilities2_server/include/capabilities2_server/utils/sql_safe.hpp similarity index 100% rename from capabilities2_server/include/capabilities2_server/sanitizer/sql_safe.hpp rename to capabilities2_server/include/capabilities2_server/utils/sql_safe.hpp 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