diff --git a/capabilities2_fabric/CMakeLists.txt b/capabilities2_fabric/CMakeLists.txt index e7f275c..9faebc0 100644 --- a/capabilities2_fabric/CMakeLists.txt +++ b/capabilities2_fabric/CMakeLists.txt @@ -15,6 +15,8 @@ find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_action REQUIRED) find_package(capabilities2_msgs REQUIRED) +find_package(bondcpp REQUIRED) +find_package(backward_ros REQUIRED) # Locate the static version of tinyxml2 find_library(TINYXML2_LIB NAMES tinyxml2 PATHS /usr/local/lib NO_DEFAULT_PATH) @@ -38,6 +40,7 @@ target_link_libraries(${PROJECT_NAME} ament_target_dependencies(${PROJECT_NAME} rclcpp rclcpp_action + bondcpp capabilities2_msgs ) diff --git a/capabilities2_fabric/docs/waypoint_runner_ex1.md b/capabilities2_fabric/docs/waypoint_runner_ex1.md index 555c773..4e978e8 100644 --- a/capabilities2_fabric/docs/waypoint_runner_ex1.md +++ b/capabilities2_fabric/docs/waypoint_runner_ex1.md @@ -2,26 +2,7 @@ ### Dependencies -This example uses nav2 stack and turtlebot3. - -Run the following commands to install nav2 stack - -```bash -sudo apt install ros-humble-navigation2 ros-humble-nav2-bringup -``` - -Run the following commands to install turtlebot3 - -```bash -sudo apt install ros-humble-turtlebot3* -``` - -Clone the nav_stack default configuration and launch files to the same workspace if its not already availabe in the workspace. Capabilities2 Nav2 Runners are dependent on this package. - -```bash -cd src -git clone https://github.com/CollaborativeRoboticsLab/nav_stack.git -``` +This example uses nav2 stack and turtlebot3. Follow instructions from [Nav2 Dependency Installation](../../docs/nav2_setup.md) to setup nav stack. ### Plan selection diff --git a/capabilities2_fabric/docs/waypoint_runner_ex2.md b/capabilities2_fabric/docs/waypoint_runner_ex2.md index 01121e9..8a49adb 100644 --- a/capabilities2_fabric/docs/waypoint_runner_ex2.md +++ b/capabilities2_fabric/docs/waypoint_runner_ex2.md @@ -2,26 +2,7 @@ ### Dependencies -This example uses nav2 stack and turtlebot3. - -Run the following commands to install nav2 stack - -```bash -sudo apt install ros-humble-navigation2 ros-humble-nav2-bringup -``` - -Run the following commands to install turtlebot3 - -```bash -sudo apt install ros-humble-turtlebot3* -``` - -Clone the nav_stack default configuration and launch files to the same workspace if its not already availabe in the workspace. Capabilities2 Nav2 Runners are dependent on this package. - -```bash -cd src -git clone https://github.com/CollaborativeRoboticsLab/nav_stack.git -``` +This example uses nav2 stack and turtlebot3. Follow instructions from [Nav2 Dependency Installation](../../docs/nav2_setup.md) to setup nav stack. ### Plan selection diff --git a/capabilities2_fabric/include/capabilities2_executor/capabilities_fabric.hpp b/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric.hpp similarity index 54% rename from capabilities2_fabric/include/capabilities2_executor/capabilities_fabric.hpp rename to capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric.hpp index 0675617..3deec48 100644 --- a/capabilities2_fabric/include/capabilities2_executor/capabilities_fabric.hpp +++ b/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric.hpp @@ -8,7 +8,8 @@ #include #include -#include +#include +#include #include @@ -75,73 +76,21 @@ class CapabilitiesFabric : public rclcpp::Node establish_bond_client_ = this->create_client("/capabilities/establish_bond"); use_capability_client_ = this->create_client("/capabilities/use_capability"); free_capability_client_ = this->create_client("/capabilities/free_capability"); - trigger_capability_client_ = this->create_client("/capabilities/trigger_capability"); - configure_capability_client_ = this->create_client("/capabilities/configure_capability"); + trig_capability_client_ = this->create_client("/capabilities/trigger_capability"); + conf_capability_client_ = this->create_client("/capabilities/configure_capability"); // Wait for services to become available - while (!get_interfaces_client_->wait_for_service(std::chrono::seconds(1))) - { - RCLCPP_ERROR(this->get_logger(), "/capabilities/get_interfaces not available"); - rclcpp::shutdown(); - return; - } - RCLCPP_INFO(this->get_logger(), "/capabilities/get_interfaces connected"); - - while (!get_sem_interf_client_->wait_for_service(std::chrono::seconds(1))) - { - RCLCPP_ERROR(this->get_logger(), "/capabilities/get_semantic_interfaces not available"); - rclcpp::shutdown(); - return; - } - RCLCPP_INFO(this->get_logger(), "/capabilities/get_semantic_interfaces connected"); - - while (!get_providers_client_->wait_for_service(std::chrono::seconds(1))) - { - RCLCPP_ERROR(this->get_logger(), "/capabilities/get_providers not available"); - rclcpp::shutdown(); - return; - } - RCLCPP_INFO(this->get_logger(), "/capabilities/get_providers connected"); - - while (!establish_bond_client_->wait_for_service(std::chrono::seconds(1))) - { - RCLCPP_ERROR(this->get_logger(), "/capabilities/establish_bond not available"); - rclcpp::shutdown(); - return; - } - RCLCPP_INFO(this->get_logger(), "/capabilities/establish_bond connected"); - - while (!use_capability_client_->wait_for_service(std::chrono::seconds(1))) - { - RCLCPP_ERROR(this->get_logger(), "/capabilities/use_capability not available"); - rclcpp::shutdown(); - return; - } - RCLCPP_INFO(this->get_logger(), "/capabilities/use_capability connected"); - - while (!free_capability_client_->wait_for_service(std::chrono::seconds(1))) - { - RCLCPP_ERROR(this->get_logger(), "/capabilities/free_capability not available"); - rclcpp::shutdown(); - return; - } - RCLCPP_INFO(this->get_logger(), "/capabilities/free_capability connected"); - - while (!trigger_capability_client_->wait_for_service(std::chrono::seconds(1))) - { - RCLCPP_ERROR(this->get_logger(), "/capabilities/trigger_capability not available"); - rclcpp::shutdown(); - return; - } - RCLCPP_INFO(this->get_logger(), "/capabilities/trigger_capability connected"); - - while (!configure_capability_client_->wait_for_service(std::chrono::seconds(1))) - { - RCLCPP_ERROR(this->get_logger(), "/capabilities/configure_capability not available"); - rclcpp::shutdown(); - return; - } - RCLCPP_INFO(this->get_logger(), "/capabilities/configure_capability connected"); + check_service(!get_interfaces_client_->wait_for_service(std::chrono::seconds(1)), "/capabilities/get_interfaces"); + check_service(!get_sem_interf_client_->wait_for_service(std::chrono::seconds(1)), "/capabilities/get_semantic_interfaces"); + check_service(!get_providers_client_->wait_for_service(std::chrono::seconds(1)), "/capabilities/get_providers"); + check_service(!establish_bond_client_->wait_for_service(std::chrono::seconds(1)), "/capabilities/establish_bond"); + check_service(!use_capability_client_->wait_for_service(std::chrono::seconds(1)), "/capabilities/use_capability"); + check_service(!free_capability_client_->wait_for_service(std::chrono::seconds(1)), "/capabilities/free_capability"); + check_service(!trig_capability_client_->wait_for_service(std::chrono::seconds(1)), "/capabilities/trigger_capability"); + check_service(!conf_capability_client_->wait_for_service(std::chrono::seconds(1)), "/capabilities/configure_capability"); + + feedback_msg = std::make_shared(); + result_msg = std::make_shared(); } private: @@ -164,7 +113,7 @@ class CapabilitiesFabric : public rclcpp::Node // check if the file parsing failed if (xml_status != tinyxml2::XMLError::XML_SUCCESS) { - RCLCPP_INFO(this->get_logger(), "Parsing the plan from goal message failed"); + RCLCPP_ERROR(this->get_logger(), "Parsing the plan from goal message failed"); return rclcpp_action::GoalResponse::REJECT; } @@ -181,7 +130,7 @@ class CapabilitiesFabric : public rclcpp::Node */ rclcpp_action::CancelResponse handle_cancel(const std::shared_ptr goal_handle) { - RCLCPP_INFO(this->get_logger(), "Received the request to cancel the plan"); + RCLCPP_ERROR(this->get_logger(), "Received the request to cancel the plan"); (void)goal_handle; return rclcpp_action::CancelResponse::ACCEPT; @@ -202,7 +151,7 @@ class CapabilitiesFabric : public rclcpp::Node */ void execution(const std::shared_ptr goal_handle) { - RCLCPP_INFO(this->get_logger(), "Execution started"); + print_and_feedback(goal_handle, "Execution started", false); expected_providers_ = 0; completed_providers_ = 0; @@ -224,42 +173,29 @@ class CapabilitiesFabric : public rclcpp::Node */ void getInterfaces(const std::shared_ptr goal_handle) { - auto feedback = std::make_shared(); - - feedback->progress = "Requesting Interface information"; - goal_handle->publish_feedback(feedback); - - RCLCPP_INFO(this->get_logger(), "Requesting Interface information"); + print_and_feedback(goal_handle, "Requesting Interface information", false); auto request_interface = std::make_shared(); // request data from the server - auto result_future = - get_interfaces_client_->async_send_request(request_interface, [this, goal_handle, feedback](GetInterfacesClient::SharedFuture future) { - auto result = std::make_shared(); - - if (!future.valid()) - { - RCLCPP_ERROR(this->get_logger(), "Failed to get Interface information"); - - result->success = false; - result->message = "Failed to get Interface information"; - goal_handle->abort(result); - - RCLCPP_INFO(this->get_logger(), "Server Execution Cancelled"); - return; - } + auto result_future = get_interfaces_client_->async_send_request(request_interface, [this, goal_handle](GetInterfacesClient::SharedFuture future) { + auto result = std::make_shared(); - auto response = future.get(); - RCLCPP_INFO(this->get_logger(), "Received Interface information"); + if (!future.valid()) + { + print_and_result(goal_handle, "Failed to get Interface information. Server execution cancelled", false); + return; + } - expected_interfaces_ = response->interfaces.size(); + auto response = future.get(); + expected_interfaces_ = response->interfaces.size(); - RCLCPP_INFO(this->get_logger(), "Requsting Semantic Interface information for %d interfaces", expected_interfaces_); + status = "Received Interfaces. Requsting " + std::to_string(expected_interfaces_) + " semantic interface information"; + print_and_feedback(goal_handle, status, false); - // Request each interface recursively for Semantic interfaces - getSemanticInterfaces(response->interfaces, goal_handle); - }); + // Request each interface recursively for Semantic interfaces + getSemanticInterfaces(response->interfaces, goal_handle); + }); } /** @@ -267,30 +203,20 @@ class CapabilitiesFabric : public rclcpp::Node */ void getSemanticInterfaces(const std::vector& interfaces, const std::shared_ptr goal_handle) { - auto feedback = std::make_shared(); - std::string requested_interface = interfaces[completed_interfaces_]; - RCLCPP_INFO(this->get_logger(), ""); - feedback->progress = "Requesting semantic interfaces for " + requested_interface; - goal_handle->publish_feedback(feedback); - - RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); + status = "Requesting semantic interfaces for " + requested_interface; + print_and_feedback(goal_handle, status, true); auto request_semantic = std::make_shared(); request_semantic->interface = requested_interface; // request semantic interface from the server auto result_semantic_future = get_sem_interf_client_->async_send_request( - request_semantic, [this, goal_handle, feedback, interfaces, requested_interface](GetSemanticInterfacesClient::SharedFuture future) { + request_semantic, [this, goal_handle, interfaces, requested_interface](GetSemanticInterfacesClient::SharedFuture future) { if (!future.valid()) { - auto result = std::make_shared(); - result->success = false; - result->message = "Failed to get Semantic Interface information"; - goal_handle->abort(result); - - RCLCPP_INFO(this->get_logger(), "Failed to get Semantic Interface information. Server Execution Cancelled"); + print_and_result(goal_handle, "Failed to get Semantic Interface information. Server execution cancelled", false); return; } @@ -305,10 +231,9 @@ class CapabilitiesFabric : public rclcpp::Node interface_list.push_back(semantic_interface); is_semantic_list.push_back(true); - feedback->progress = std::to_string(completed_interfaces_) + "/" + std::to_string(expected_interfaces_) + " : Received " + - semantic_interface + " for " + requested_interface + ". So added " + semantic_interface; - goal_handle->publish_feedback(feedback); - RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); + status = (completed_interfaces_) + "/" + std::to_string(expected_interfaces_) + " : Received " + semantic_interface + " for " + + requested_interface + ". So added " + semantic_interface; + print_and_feedback(goal_handle, status, false); } } // if no semantic interfaces are availble for a given interface, add the interface instead @@ -317,11 +242,9 @@ class CapabilitiesFabric : public rclcpp::Node interface_list.push_back(requested_interface); is_semantic_list.push_back(false); - feedback->progress = std::to_string(completed_interfaces_) + "/" + std::to_string(expected_interfaces_) + " : Received none for " + - requested_interface + ". So added " + requested_interface; - goal_handle->publish_feedback(feedback); - - RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); + status = std::to_string(completed_interfaces_) + "/" + std::to_string(expected_interfaces_) + " : Received none for " + + requested_interface + ". So added " + requested_interface; + print_and_feedback(goal_handle, status, false); } if (completed_interfaces_ != expected_interfaces_) @@ -331,12 +254,12 @@ class CapabilitiesFabric : public rclcpp::Node } else { - RCLCPP_INFO(this->get_logger(), ""); - RCLCPP_INFO(this->get_logger(), "Received all requested Interface information"); + print_and_feedback(goal_handle, "Received all requested Interface information", true); expected_providers_ = interface_list.size(); - RCLCPP_INFO(this->get_logger(), "Requsting Provider information for %d providers", expected_providers_); + status = "Requsting Provider information for " + std::to_string(expected_providers_) + " providers"; + print_and_feedback(goal_handle, status, false); // request providers from the interfaces in the interfaces_list getProvider(interface_list, is_semantic_list, goal_handle); @@ -354,12 +277,8 @@ class CapabilitiesFabric : public rclcpp::Node std::string requested_interface = interfaces[completed_providers_]; bool semantic_flag = is_semantic[completed_providers_]; - auto feedback = std::make_shared(); - - RCLCPP_INFO(this->get_logger(), ""); - feedback->progress = "Requesting provider for " + requested_interface; - goal_handle->publish_feedback(feedback); - RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); + status = "Requesting provider for " + requested_interface; + print_and_feedback(goal_handle, status, true); auto request_providers = std::make_shared(); @@ -368,15 +287,11 @@ class CapabilitiesFabric : public rclcpp::Node request_providers->include_semantic = semantic_flag; auto result_providers_future = get_providers_client_->async_send_request( - request_providers, [this, is_semantic, requested_interface, interfaces, goal_handle, feedback](GetProvidersClient::SharedFuture future) { + request_providers, [this, is_semantic, requested_interface, interfaces, goal_handle](GetProvidersClient::SharedFuture future) { if (!future.valid()) { - auto result = std::make_shared(); - result->success = false; - result->message = "Did not retrieve providers for interface: " + requested_interface; - goal_handle->abort(result); - - RCLCPP_ERROR(this->get_logger(), result->message.c_str()); + status = "Did not retrieve providers for interface: " + requested_interface; + print_and_result(goal_handle, status, false); return; } @@ -388,10 +303,9 @@ class CapabilitiesFabric : public rclcpp::Node // add defualt provider to the list providers_list.push_back(response->default_provider); - feedback->progress = std::to_string(completed_providers_) + "/" + std::to_string(expected_providers_) + " : Received " + - response->default_provider + " for " + requested_interface + ". So added " + response->default_provider; - goal_handle->publish_feedback(feedback); - RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); + status = std::to_string(completed_providers_) + "/" + std::to_string(expected_providers_) + " : Received " + response->default_provider + + " for " + requested_interface + ". So added " + response->default_provider; + print_and_feedback(goal_handle, status, false); } // add additional providers to the list if available @@ -400,18 +314,16 @@ class CapabilitiesFabric : public rclcpp::Node for (const auto& provider : response->providers) { providers_list.push_back(provider); - feedback->progress = std::to_string(completed_providers_) + "/" + std::to_string(expected_providers_) + " : Received and added " + - provider + " for " + requested_interface; - goal_handle->publish_feedback(feedback); - RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); + + status = std::to_string(completed_providers_) + "/" + std::to_string(expected_providers_) + " : Received and added " + provider + + " for " + requested_interface; + print_and_feedback(goal_handle, status, false); } } else { - feedback->progress = - std::to_string(completed_providers_) + "/" + std::to_string(expected_providers_) + " : No providers for " + requested_interface; - goal_handle->publish_feedback(feedback); - RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); + status = std::to_string(completed_providers_) + "/" + std::to_string(expected_providers_) + " : No providers for " + requested_interface; + print_and_feedback(goal_handle, status, false); } // Check if all expected calls are completed before calling verify_plan @@ -422,12 +334,7 @@ class CapabilitiesFabric : public rclcpp::Node } else { - auto feedback = std::make_shared(); - - RCLCPP_INFO(this->get_logger(), ""); - feedback->progress = "All requested interface, semantic interface and provider data recieved"; - goal_handle->publish_feedback(feedback); - RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); + print_and_feedback(goal_handle, "All requested interface, semantic interface and provider data recieved", true); verify_and_continue(goal_handle); } @@ -440,23 +347,17 @@ class CapabilitiesFabric : public rclcpp::Node */ void verify_and_continue(const std::shared_ptr goal_handle) { - auto feedback = std::make_shared(); - feedback->progress = "Verifying the plan"; - goal_handle->publish_feedback(feedback); - RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); + print_and_feedback(goal_handle, "Verifying the plan", false); // verify the plan if (!verify_plan(goal_handle)) { - feedback->progress = "Plan verification failed"; - goal_handle->publish_feedback(feedback); - RCLCPP_ERROR(this->get_logger(), feedback->progress.c_str()); - - auto result = std::make_shared(); + print_and_feedback(goal_handle, "Plan verification failed", false); if (rejected_list.size() > 0) { // TODO: improve with error codes + auto result = std::make_shared(); result->success = false; result->message = "Plan verification failed. There are mismatched events"; @@ -472,32 +373,23 @@ class CapabilitiesFabric : public rclcpp::Node else { // TODO: improve with error codes - result->success = false; - result->message = "Plan verification failed."; - goal_handle->abort(result); - RCLCPP_ERROR(this->get_logger(), "Server Execution Cancelled"); + print_and_result(goal_handle, "Plan verification failed. Server Execution Cancelled.", false); } RCLCPP_ERROR(this->get_logger(), "Server Execution Cancelled"); } - feedback->progress = "Plan verification successful"; - goal_handle->publish_feedback(feedback); - RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); + print_and_feedback(goal_handle, "Plan verification successful", false); // extract the plan from the XMLDocument tinyxml2::XMLElement* plan = capabilities2_xml_parser::get_plan(document); - feedback->progress = "Plan conversion successful"; - goal_handle->publish_feedback(feedback); - RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); + print_and_feedback(goal_handle, "Plan conversion successful", false); // Extract the connections from the plan capabilities2_xml_parser::extract_connections(plan, connection_map); - feedback->progress = "Connection extraction successful"; - goal_handle->publish_feedback(feedback); - RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); + print_and_feedback(goal_handle, "Connection extraction successful", false); // estasblish the bond with the server establish_bond(goal_handle); @@ -520,37 +412,25 @@ class CapabilitiesFabric : public rclcpp::Node // verify whether document got 'plan' tags if (!capabilities2_xml_parser::check_plan_tag(document)) { - result->success = false; - result->message = "Execution plan is not compatible. Please recheck and update"; - goal_handle->abort(result); - RCLCPP_ERROR(this->get_logger(), result->message.c_str()); + print_and_result(goal_handle, "Execution plan is not compatible. Please recheck and update", false); return false; } - feedback->progress = "'Plan' tag checking successful"; - goal_handle->publish_feedback(feedback); - RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); + print_and_feedback(goal_handle, "'Plan' tag checking successful", false); // extract the components within the 'plan' tags tinyxml2::XMLElement* plan = capabilities2_xml_parser::get_plan(document); - feedback->progress = "Plan extraction complete"; - goal_handle->publish_feedback(feedback); - RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); + print_and_feedback(goal_handle, "Plan extraction complete", false); // verify whether the plan is valid if (!capabilities2_xml_parser::check_tags(this->get_logger(), plan, interface_list, providers_list, control_tag_list, rejected_list)) { - result->success = false; - result->message = "Execution plan is faulty. Please recheck and update"; - goal_handle->abort(result); - RCLCPP_ERROR(this->get_logger(), result->message.c_str()); + print_and_result(goal_handle, "Execution plan is faulty. Please recheck and update", false); return false; } - feedback->progress = "Checking tags successful"; - goal_handle->publish_feedback(feedback); - RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); + print_and_feedback(goal_handle, "Checking tags successful", false); return true; } @@ -560,129 +440,100 @@ class CapabilitiesFabric : public rclcpp::Node */ void establish_bond(const std::shared_ptr goal_handle) { - auto feedback = std::make_shared(); - feedback->progress = "Requesting bond id"; - goal_handle->publish_feedback(feedback); - - RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); + print_and_feedback(goal_handle, "Requesting bond id", false); // create bond establishing server request auto request_bond = std::make_shared(); // send the request - auto result_future = - establish_bond_client_->async_send_request(request_bond, [this, goal_handle, feedback](EstablishBondClient::SharedFuture future) { - if (!future.valid()) - { - RCLCPP_ERROR(this->get_logger(), "Failed to receive the bond id"); - - auto result = std::make_shared(); - result->success = false; - result->message = "Failed to retrieve the bond id"; - RCLCPP_ERROR(this->get_logger(), "Server Execution Cancelled"); + auto result_future = establish_bond_client_->async_send_request(request_bond, [this, goal_handle](EstablishBondClient::SharedFuture future) { + if (!future.valid()) + { + print_and_result(goal_handle, "Failed to retrieve the bond id. Server execution cancelled", false); + return; + } - goal_handle->abort(result); - return; - } + auto response = future.get(); + bond_id_ = response->bond_id; - auto response = future.get(); - bond_id = response->bond_id; + status = "Received the bond id : " + bond_id_; + print_and_feedback(goal_handle, status, false); - feedback->progress = "Received the bond id : " + bond_id; - goal_handle->publish_feedback(feedback); - RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); + bond_client_ = std::make_unique(shared_from_this(), bond_id_); + bond_client_->start(); - expected_capabilities_ = connection_map.size(); + expected_capabilities_ = connection_map.size(); - RCLCPP_INFO(this->get_logger(), "Requsting use (start) of %d capabilities", expected_capabilities_); + status = "Requsting start of " + std::to_string(expected_capabilities_) + "capabilities"; + print_and_feedback(goal_handle, status, false); - use_capability(connection_map, goal_handle); - }); + use_capability(connection_map, goal_handle); + }); } /** * @brief Request use of capability from capabilities2 server * - * @param capability capability name to be started + * @param capabilities capability list to be started * @param provider provider of the capability - * @param bond_id bond_id for the capability */ void use_capability(std::map& capabilities, const std::shared_ptr goal_handle) { - auto feedback = std::make_shared(); - std::string capability = capabilities[completed_capabilities_].source.runner; std::string provider = capabilities[completed_capabilities_].source.provider; auto request_use = std::make_shared(); request_use->capability = capability; request_use->preferred_provider = provider; - request_use->bond_id = bond_id; + request_use->bond_id = bond_id_; - feedback->progress = - "Using (starting) capability of Node " + std::to_string(completed_capabilities_) + " named " + capabilities[completed_capabilities_].source.runner; - goal_handle->publish_feedback(feedback); - RCLCPP_INFO(this->get_logger(), ""); - RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); + status = "Starting capability of Node " + std::to_string(completed_capabilities_) + " : " + capabilities[completed_capabilities_].source.runner; + print_and_feedback(goal_handle, status, true); // send the request - auto result_future = use_capability_client_->async_send_request(request_use, [this, goal_handle, feedback, capability, - provider](UseCapabilityClient::SharedFuture future) { - if (!future.valid()) - { - auto result = std::make_shared(); - result->success = false; - result->message = "Failed to Use capability " + capability + " from " + provider; - goal_handle->abort(result); - - RCLCPP_ERROR(this->get_logger(), result->message.c_str()); - RCLCPP_ERROR(this->get_logger(), "Server Execution Cancelled"); - - // release all capabilities that were used since not all started successfully - for (const auto& [key, value] : connection_map) - { - feedback->progress = "Freeing capability of Node " + std::to_string(key) + " named " + value.source.runner; - goal_handle->publish_feedback(feedback); - RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); + auto result_future = + use_capability_client_->async_send_request(request_use, [this, goal_handle, capability, provider](UseCapabilityClient::SharedFuture future) { + if (!future.valid()) + { + status = "Failed to Use capability " + capability + " from " + provider + ". Server Execution Cancelled"; + print_and_result(goal_handle, status, false); - free_capability(value.source.runner, goal_handle); - } + // release all capabilities that were used since not all started successfully + for (const auto& [key, value] : connection_map) + { + status = "Freeing capability of Node " + std::to_string(key) + " named " + value.source.runner; + print_and_feedback(goal_handle, status, false); - return; - } + free_capability(value.source.runner, goal_handle); + } - completed_capabilities_++; + return; + } - auto response = future.get(); + completed_capabilities_++; - feedback->progress = std::to_string(completed_capabilities_) + "/" + std::to_string(expected_capabilities_) + " : Capability use (start) succeessful"; - goal_handle->publish_feedback(feedback); - RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); + auto response = future.get(); - // Check if all expected calls are completed before calling verify_plan - if (completed_capabilities_ == expected_capabilities_) - { - RCLCPP_INFO(this->get_logger(), ""); - feedback->progress = "All requested capabilities have been used"; - goal_handle->publish_feedback(feedback); - RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); + status = std::to_string(completed_capabilities_) + "/" + std::to_string(expected_capabilities_) + " : start succeessful"; + print_and_feedback(goal_handle, status, true); - feedback->progress = "Configuring the capabilities with events"; - goal_handle->publish_feedback(feedback); - RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); + // Check if all expected calls are completed before calling verify_plan + if (completed_capabilities_ == expected_capabilities_) + { + print_and_feedback(goal_handle, "All requested capabilities have been started. Configuring the capabilities with events", true); - expected_configurations_ = connection_map.size(); + expected_configurations_ = connection_map.size(); - RCLCPP_INFO(this->get_logger(), ""); - RCLCPP_INFO(this->get_logger(), "Requsting capability configuration for %d capabilities", expected_configurations_); + status = "Requsting capability configuration for " + std::to_string(expected_configurations_) + "capabilities"; + print_and_feedback(goal_handle, status, true); - configure_capabilities(connection_map, goal_handle); - } - else - { - use_capability(connection_map, goal_handle); - } - }); + configure_capabilities(connection_map, goal_handle); + } + else + { + use_capability(connection_map, goal_handle); + } + }); } /** @@ -692,30 +543,26 @@ class CapabilitiesFabric : public rclcpp::Node */ void free_capability(const std::string& capability, const std::shared_ptr goal_handle) { - auto feedback = std::make_shared(); - auto request_free = std::make_shared(); request_free->capability = capability; - request_free->bond_id = bond_id; + request_free->bond_id = bond_id_; // send the request - auto result_future = free_capability_client_->async_send_request( - request_free, [this, goal_handle, capability, feedback](FreeCapabilityClient::SharedFuture future) { + auto result_future = + free_capability_client_->async_send_request(request_free, [this, goal_handle, capability](FreeCapabilityClient::SharedFuture future) { if (!future.valid()) { - auto result = std::make_shared(); - result->success = false; - result->message = "Failed to free capability " + capability; - goal_handle->abort(result); - - RCLCPP_ERROR(this->get_logger(), result->message.c_str()); + status = "Failed to free capability " + capability; + print_and_result(goal_handle, status, false); return; } auto response = future.get(); - feedback->progress = "Successfully freed capability " + capability; - goal_handle->publish_feedback(feedback); - RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); + + status = "Successfully freed capability " + capability; + print_and_feedback(goal_handle, status, true); + + bond_client_->stop(); }); } @@ -724,14 +571,11 @@ class CapabilitiesFabric : public rclcpp::Node */ void configure_capabilities(std::map& capabilities, const std::shared_ptr goal_handle) { - auto feedback = std::make_shared(); auto request_configure = std::make_shared(); - RCLCPP_INFO(this->get_logger(), ""); - feedback->progress = "Configuring capability of Node " + std::to_string(completed_configurations_) + " named " + - capabilities[completed_configurations_].source.runner; - goal_handle->publish_feedback(feedback); - RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); + status = "Configuring capability of Node " + std::to_string(completed_configurations_) + " named " + + capabilities[completed_configurations_].source.runner; + print_and_feedback(goal_handle, status, true); if (capabilities2_xml_parser::convert_to_string(capabilities[completed_configurations_].source.parameters, request_configure->source.parameters)) { @@ -810,17 +654,12 @@ class CapabilitiesFabric : public rclcpp::Node std::string source_capability = capabilities[completed_configurations_].source.runner; // send the request - auto result_future = configure_capability_client_->async_send_request( - request_configure, [this, goal_handle, source_capability, feedback](ConfigureCapabilityClient::SharedFuture future) { + auto result_future = conf_capability_client_->async_send_request( + request_configure, [this, goal_handle, source_capability](ConfigureCapabilityClient::SharedFuture future) { if (!future.valid()) { - auto result = std::make_shared(); - result->success = false; - result->message = "Failed to configure capability :" + source_capability; - goal_handle->abort(result); - - RCLCPP_ERROR(this->get_logger(), result->message.c_str()); - RCLCPP_ERROR(this->get_logger(), "Server Execution Cancelled"); + status = "Failed to configure capability :" + source_capability + ". Server execution cancelled"; + print_and_result(goal_handle, status, false); return; } @@ -828,24 +667,14 @@ class CapabilitiesFabric : public rclcpp::Node auto response = future.get(); - feedback->progress = std::to_string(completed_configurations_) + "/" + std::to_string(expected_configurations_) + - " : Successfully configured capability : " + source_capability; - goal_handle->publish_feedback(feedback); - RCLCPP_INFO(this->get_logger(), ""); - RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); + status = std::to_string(completed_configurations_) + "/" + std::to_string(expected_configurations_) + + " : Successfully configured capability : " + source_capability; + print_and_feedback(goal_handle, status, true); // Check if all expected calls are completed before calling verify_plan if (completed_configurations_ == expected_configurations_) { - RCLCPP_INFO(this->get_logger(), ""); - feedback->progress = "All requested capabilities have been configured"; - goal_handle->publish_feedback(feedback); - RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); - - RCLCPP_INFO(this->get_logger(), ""); - feedback->progress = "Triggering the first capability"; - goal_handle->publish_feedback(feedback); - RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); + print_and_feedback(goal_handle, "All requested capabilities have been configured. Triggering the first capability", true); trigger_first_node(goal_handle); } @@ -861,8 +690,6 @@ class CapabilitiesFabric : public rclcpp::Node */ void trigger_first_node(const std::shared_ptr goal_handle) { - auto feedback = std::make_shared(); - auto request_trigger = std::make_shared(); std::string parameter_string; @@ -872,25 +699,65 @@ class CapabilitiesFabric : public rclcpp::Node // send the request auto result_future = - trigger_capability_client_->async_send_request(request_trigger, [this, goal_handle, feedback](TriggerCapabilityClient::SharedFuture future) { + trig_capability_client_->async_send_request(request_trigger, [this, goal_handle](TriggerCapabilityClient::SharedFuture future) { if (!future.valid()) { - auto result = std::make_shared(); - result->success = false; - result->message = "Failed to trigger capability " + connection_map[0].source.runner; - goal_handle->abort(result); - - RCLCPP_ERROR(this->get_logger(), result->message.c_str()); + status = "Failed to trigger capability " + connection_map[0].source.runner; + print_and_result(goal_handle, status, false); return; } auto response = future.get(); - feedback->progress = "Successfully triggered capability " + connection_map[0].source.runner; - goal_handle->publish_feedback(feedback); - RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); + + status = "Successfully triggered capability " + connection_map[0].source.runner; + print_and_feedback(goal_handle, status, false); + + print_and_result(goal_handle, "Successfully launched capabilities2 fabric", true); }); } + void check_service(bool wait_for_logic, const std::string& service_name) + { + while (wait_for_logic) + { + std::string failed = service_name + " not available"; + RCLCPP_ERROR(this->get_logger(), failed.c_str()); + rclcpp::shutdown(); + return; + } + + std::string success = service_name + " connected"; + RCLCPP_INFO(this->get_logger(), success.c_str()); + } + + void print_and_feedback(const std::shared_ptr goal_handle, const std::string& text, bool newline) + { + feedback_msg->progress = text; + goal_handle->publish_feedback(feedback_msg); + + if (newline) + RCLCPP_INFO(this->get_logger(), ""); + + RCLCPP_INFO(this->get_logger(), feedback_msg->progress.c_str()); + } + + void print_and_result(const std::shared_ptr goal_handle, const std::string& text, bool success) + { + result_msg->success = success; + result_msg->message = text; + + if (success) + { + goal_handle->succeed(result_msg); + RCLCPP_INFO(this->get_logger(), result_msg->message.c_str()); + } + else + { + goal_handle->abort(result_msg); + RCLCPP_ERROR(this->get_logger(), result_msg->message.c_str()); + } + } + private: /** File Path link */ std::string plan_file_path; @@ -910,8 +777,14 @@ class CapabilitiesFabric : public rclcpp::Node int expected_configurations_; int completed_configurations_; - /** Bond ID between capabilities server and this client */ - std::string bond_id; + /** status message string */ + std::string status; + + /** Bond id */ + std::string bond_id_; + + /** Bond between capabilities server and this client */ + std::shared_ptr bond_client_; /** XML Document */ tinyxml2::XMLDocument document; @@ -934,6 +807,12 @@ class CapabilitiesFabric : public rclcpp::Node /** Invalid events list */ std::vector rejected_list; + /** Feedback message for plan action server*/ + std::shared_ptr feedback_msg; + + /** Result message for plan action server*/ + std::shared_ptr result_msg; + /** action server that exposes executor*/ std::shared_ptr> planner_server_; @@ -959,8 +838,8 @@ class CapabilitiesFabric : public rclcpp::Node FreeCapabilityClient::SharedPtr free_capability_client_; /** configure an selected capability */ - ConfigureCapabilityClient::SharedPtr configure_capability_client_; + ConfigureCapabilityClient::SharedPtr conf_capability_client_; /** trigger an selected capability */ - TriggerCapabilityClient::SharedPtr trigger_capability_client_; + TriggerCapabilityClient::SharedPtr trig_capability_client_; }; \ No newline at end of file diff --git a/capabilities2_fabric/include/capabilities2_executor/capabilities_file_parser.hpp b/capabilities2_fabric/include/capabilities2_fabric/capabilities_file_parser.hpp similarity index 88% rename from capabilities2_fabric/include/capabilities2_executor/capabilities_file_parser.hpp rename to capabilities2_fabric/include/capabilities2_fabric/capabilities_file_parser.hpp index c576712..d7614e9 100644 --- a/capabilities2_fabric/include/capabilities2_executor/capabilities_file_parser.hpp +++ b/capabilities2_fabric/include/capabilities2_fabric/capabilities_file_parser.hpp @@ -10,7 +10,7 @@ #include #include -#include +#include #include /** @@ -38,14 +38,20 @@ class CapabilitiesFileParser : public rclcpp::Node // Create the action client for capabilities_fabric after the node is fully constructed this->client_ptr_ = rclcpp_action::create_client(shared_from_this(), "/capabilities_fabric"); - // Set up a timer to call send_goal after a short delay - this->timer_ = this->create_wall_timer(std::chrono::milliseconds(500), std::bind(&CapabilitiesFileParser::send_goal, this)); + if (!this->client_ptr_->wait_for_action_server(std::chrono::seconds(5))) + { + RCLCPP_ERROR(this->get_logger(), "Action server not available after waiting"); + rclcpp::shutdown(); + return; + } + + RCLCPP_INFO(this->get_logger(), "Sucessfully connected to the capabilities_fabric action server"); + + send_goal(); } void send_goal() { - this->timer_->cancel(); - // try to load the file tinyxml2::XMLError xml_status = document.LoadFile(plan_file_path.c_str()); @@ -58,20 +64,11 @@ class CapabilitiesFileParser : public rclcpp::Node RCLCPP_INFO(this->get_logger(), "Plan loaded from : %s", plan_file_path.c_str()); - if (!this->client_ptr_->wait_for_action_server(std::chrono::seconds(5))) - { - RCLCPP_ERROR(this->get_logger(), "Action server not available after waiting"); - rclcpp::shutdown(); - return; - } - - RCLCPP_INFO(this->get_logger(), "Sucessfully connected to the capabilities_fabric action server"); - auto goal_msg = capabilities2_msgs::action::Plan::Goal(); capabilities2_xml_parser::convert_to_string(document, goal_msg.plan); - RCLCPP_INFO(this->get_logger(), "Following plan was read:\n\n%s ", goal_msg.plan.c_str()); + RCLCPP_INFO(this->get_logger(), "Following plan was loaded :\n\n%s ", goal_msg.plan.c_str()); RCLCPP_INFO(this->get_logger(), "Sending goal to the capabilities_fabric action server"); @@ -110,7 +107,7 @@ class CapabilitiesFileParser : public rclcpp::Node if (result.result->success) { - RCLCPP_ERROR(this->get_logger(), "Plan executed successfully"); + RCLCPP_INFO(this->get_logger(), "Plan executed successfully"); } else { @@ -124,8 +121,6 @@ class CapabilitiesFileParser : public rclcpp::Node RCLCPP_ERROR(this->get_logger(), "Failed Elements : %s", failed_element.c_str()); } } - - rclcpp::shutdown(); }; this->client_ptr_->async_send_goal(goal_msg, send_goal_options); @@ -140,7 +135,4 @@ class CapabilitiesFileParser : public rclcpp::Node /** action client */ rclcpp_action::Client::SharedPtr client_ptr_; - - /** action server */ - rclcpp::TimerBase::SharedPtr timer_; }; \ No newline at end of file diff --git a/capabilities2_fabric/include/capabilities2_executor/capabilities_xml_parser.hpp b/capabilities2_fabric/include/capabilities2_fabric/capabilities_xml_parser.hpp similarity index 98% rename from capabilities2_fabric/include/capabilities2_executor/capabilities_xml_parser.hpp rename to capabilities2_fabric/include/capabilities2_fabric/capabilities_xml_parser.hpp index cd43df5..6910d60 100644 --- a/capabilities2_fabric/include/capabilities2_executor/capabilities_xml_parser.hpp +++ b/capabilities2_fabric/include/capabilities2_fabric/capabilities_xml_parser.hpp @@ -2,7 +2,7 @@ #include #include #include -#include +#include namespace capabilities2_xml_parser { @@ -68,18 +68,18 @@ bool check_plan_tag(tinyxml2::XMLDocument& document) * * @return `true` if element is not nullptr and conversion successful, `false` if element is nullptr */ -bool convert_to_string(tinyxml2::XMLElement* element, std::string& paramters) +bool convert_to_string(tinyxml2::XMLElement* element, std::string& parameters) { if (element) { tinyxml2::XMLPrinter printer; element->Accept(&printer); - paramters = printer.CStr(); + parameters = printer.CStr(); return true; } else { - paramters = ""; + parameters = ""; return false; } } diff --git a/capabilities2_fabric/include/capabilities2_fabric/utils/bond_client.hpp b/capabilities2_fabric/include/capabilities2_fabric/utils/bond_client.hpp new file mode 100644 index 0000000..b1f286a --- /dev/null +++ b/capabilities2_fabric/include/capabilities2_fabric/utils/bond_client.hpp @@ -0,0 +1,65 @@ +#include +#include + +class BondClient +{ +public: + BondClient(rclcpp::Node::SharedPtr node, const std::string& bond_id, const std::string& bonds_topic = "/capabilities/bond") + { + bonds_topic_ = bonds_topic; + bond_id_ = bond_id; + node_ = node; + } + + void start() + { + RCLCPP_INFO(node_->get_logger(), "[BondClient] creating bond to capabilities server"); + + bond_ = + std::make_unique(bonds_topic_, bond_id_, node_, std::bind(&BondClient::on_broken, this), std::bind(&BondClient::on_formed, this)); + + bond_->setHeartbeatPeriod(0.10); + bond_->setHeartbeatTimeout(10.0); + bond_->start(); + } + + void stop() + { + RCLCPP_INFO(node_->get_logger(), "[BondClient] destroying bond to capabilities server"); + + if (bond_) + { + bond_.reset(); + } + } + + ~BondClient() + { + stop(); + } + +private: + void on_formed() + { + // log bond established event + RCLCPP_INFO(node_->get_logger(), "[BondClient] bond with capabilities server formed with id: %s", bond_id_.c_str()); + } + + void on_broken() + { + // log bond established event + RCLCPP_INFO(node_->get_logger(), "[BondClient] bond with capabilities server broken with id: %s", bond_id_.c_str()); + } + + /** Ros node pointer */ + rclcpp::Node::SharedPtr node_; + + /** Bond id string */ + std::string bond_id_; + + /** Bond topic to be published */ + std::string bonds_topic_; + + /** Heart beat bond with capabilities server */ + std::shared_ptr bond_; +}; \ No newline at end of file diff --git a/capabilities2_fabric/include/capabilities2_executor/structs/connection.hpp b/capabilities2_fabric/include/capabilities2_fabric/utils/connection.hpp similarity index 100% rename from capabilities2_fabric/include/capabilities2_executor/structs/connection.hpp rename to capabilities2_fabric/include/capabilities2_fabric/utils/connection.hpp diff --git a/capabilities2_fabric/package.xml b/capabilities2_fabric/package.xml index 25f55e8..43af93a 100644 --- a/capabilities2_fabric/package.xml +++ b/capabilities2_fabric/package.xml @@ -13,6 +13,7 @@ rclcpp capabilities2_msgs rclcpp_action + backward_ros tinyxml2 diff --git a/capabilities2_fabric/plans/navigation_1.xml b/capabilities2_fabric/plans/navigation_1.xml index 7e48c39..4dbae38 100644 --- a/capabilities2_fabric/plans/navigation_1.xml +++ b/capabilities2_fabric/plans/navigation_1.xml @@ -1,6 +1,6 @@ - + \ No newline at end of file diff --git a/capabilities2_fabric/plans/navigation_2.xml b/capabilities2_fabric/plans/navigation_2.xml index e87805d..5c283d1 100644 --- a/capabilities2_fabric/plans/navigation_2.xml +++ b/capabilities2_fabric/plans/navigation_2.xml @@ -1,7 +1,7 @@ - - + + \ No newline at end of file diff --git a/capabilities2_fabric/src/capabilities_fabric.cpp b/capabilities2_fabric/src/capabilities_fabric.cpp index 006bcc2..e89ccbb 100644 --- a/capabilities2_fabric/src/capabilities_fabric.cpp +++ b/capabilities2_fabric/src/capabilities_fabric.cpp @@ -1,6 +1,6 @@ -#include +#include -int main(int argc, char* argv[]) +int main(int argc, char** argv) { rclcpp::init(argc, argv); @@ -9,6 +9,7 @@ int main(int argc, char* argv[]) // Initialize the node components after construction node->initialize(); + rclcpp::spin(node); rclcpp::shutdown(); diff --git a/capabilities2_fabric/src/capabilities_file_parser.cpp b/capabilities2_fabric/src/capabilities_file_parser.cpp index aa0d9b0..bb08cf0 100644 --- a/capabilities2_fabric/src/capabilities_file_parser.cpp +++ b/capabilities2_fabric/src/capabilities_file_parser.cpp @@ -1,4 +1,4 @@ -#include +#include int main(int argc, char* argv[]) { diff --git a/capabilities2_launch_py/capabilities2_launch_py/__init__.py b/capabilities2_launch/capabilities2_launch/__init__.py similarity index 100% rename from capabilities2_launch_py/capabilities2_launch_py/__init__.py rename to capabilities2_launch/capabilities2_launch/__init__.py diff --git a/capabilities2_launch/capabilities2_launch/capabilities2_launch.py b/capabilities2_launch/capabilities2_launch/capabilities2_launch.py new file mode 100644 index 0000000..854c232 --- /dev/null +++ b/capabilities2_launch/capabilities2_launch/capabilities2_launch.py @@ -0,0 +1,146 @@ +import os +import signal +import rclpy +from rclpy.node import Node +from capabilities2_msgs.srv import Launch +from multiprocessing.connection import Client + +class LaunchServer(Node): + def __init__(self): + super().__init__('capabilities2_launch_server') + self.address = ('localhost', 6000) + self.client = Client(self.address, authkey=b'capabilities2') + + # Service for starting a launch file + self.start_server = self.create_service( + Launch, + '/capabilities/launch/start', + self.start_request + ) + + # Service for stopping a launch file + self.stop_server = self.create_service( + Launch, + '/capabilities/launch/stop', + self.stop_request + ) + + # Service for stopping a launch file + self.stop_server = self.create_service( + Launch, + '/capabilities/launch/status', + self.status_request + ) + + self.get_logger().info('Capabilities2 LaunchServer is ready.') + + def start_request(self, request, response): + package_name = request.package_name + launch_file_name = request.launch_file_name + + self.get_logger().info(f'[{package_name}/{launch_file_name}] received start request') + + message_outgoing = {} + message_outgoing["command"] = "start" + message_outgoing["package"] = package_name + message_outgoing["launch_file"] = launch_file_name + + self.get_logger().info(f'[{package_name}/{launch_file_name}] sending start request to Launch Server') + + self.client.send(message_outgoing) + + self.get_logger().info(f'[{package_name}/{launch_file_name}] waiting for response from Launch Server') + + message_incoming = self.client.recv() + response_content = message_incoming["status"] + + self.get_logger().info(f'[{package_name}/{launch_file_name}] received response : {response_content}') + + response.status = response_content + + return response + + def stop_request(self, request, response): + package_name = request.package_name + launch_file_name = request.launch_file_name + + self.get_logger().info(f'[{package_name}/{launch_file_name}] received stop request') + + message_outgoing = {} + message_outgoing["command"] = "stop" + message_outgoing["package"] = package_name + message_outgoing["launch_file"] = launch_file_name + + self.get_logger().info(f'[{package_name}/{launch_file_name}] sending stop request to Launch Server') + + self.client.send(message_outgoing) + + self.get_logger().info(f'[{package_name}/{launch_file_name}] waiting for response from Launch Server') + + message_incoming = self.client.recv() + response_content = message_incoming["status"] + + self.get_logger().info(f'[{package_name}/{launch_file_name}] received response : {response_content}') + + response.status = response_content + + return response + + + def status_request(self, request, response): + package_name = request.package_name + launch_file_name = request.launch_file_name + + self.get_logger().info(f'[{package_name}/{launch_file_name}] received status request') + + message_outgoing = {} + message_outgoing["command"] = "status" + message_outgoing["package"] = package_name + message_outgoing["launch_file"] = launch_file_name + + self.get_logger().info(f'[{package_name}/{launch_file_name}] sending status request to Launch Server') + + self.client.send(message_outgoing) + + self.get_logger().info(f'[{package_name}/{launch_file_name}] waiting for response from Launch Server') + + message_incoming = self.client.recv() + response_content = message_incoming["status"] + + self.get_logger().info(f'[{package_name}/{launch_file_name}] received response : {response_content}') + + response.status = response_content + + return response + + def shutdown(self): + + self.get_logger().info(f'Shutting down LaunchFileServer...') + + message_outgoing = {} + message_outgoing["command"] = "exit" + message_outgoing["package"] = "" + message_outgoing["launch_file"] = "" + + self.get_logger().info(f'[sending shutdown request to Launch Server') + + self.client.send(message_outgoing) + + +def main(args=None): + rclpy.init(args=args) + + # Create the LaunchServer node + launch_server = LaunchServer() + + # Keep the node running + try: + rclpy.spin(launch_server) + except KeyboardInterrupt: + launch_server.shutdown() + finally: + rclpy.shutdown() + + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/capabilities2_launch/launch/server.launch.py b/capabilities2_launch/launch/server.launch.py new file mode 100644 index 0000000..8ce3c51 --- /dev/null +++ b/capabilities2_launch/launch/server.launch.py @@ -0,0 +1,37 @@ +''' +capabilities2_server launch file +''' + +import os +from launch import LaunchDescription +from launch.actions import ExecuteProcess +from launch_ros.actions import Node +from launch.substitutions import FindExecutable +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + """Generate launch description for capabilities2 server + + Returns: + LaunchDescription: The launch description for capabilities2 server + """ + + filePath = os.path.join(get_package_share_directory('capabilities2_launch'), 'launch_server', 'server.py') + + # create launch proxy node + launch_interface = Node( + package='capabilities2_launch', + executable='capabilities2_launch', + name='capabilities2_launch', + output='screen', + arguments=['--ros-args', '--log-level', 'info'] + ) + + server_process = ExecuteProcess(cmd=[[FindExecutable(name='python3'), ' ', filePath]], shell=True, output='screen') + + # return + return LaunchDescription([ + launch_interface, + server_process + ]) diff --git a/capabilities2_launch_py/launch_process/__init__.py b/capabilities2_launch/launch_server/__init__.py similarity index 100% rename from capabilities2_launch_py/launch_process/__init__.py rename to capabilities2_launch/launch_server/__init__.py diff --git a/capabilities2_launch_py/launch_process/launch_process.py b/capabilities2_launch/launch_server/launch_process.py similarity index 69% rename from capabilities2_launch_py/launch_process/launch_process.py rename to capabilities2_launch/launch_server/launch_process.py index 14ba22d..04af439 100644 --- a/capabilities2_launch_py/launch_process/launch_process.py +++ b/capabilities2_launch/launch_server/launch_process.py @@ -49,7 +49,7 @@ def __init__( raise RuntimeError(str(exc)) def run(self): - print(f"Launch file {self.launch_file_name} from package {self.package_name} started.") + print(f"Launch file {self.launch_file_name} from package {self.package_name} started with pid [{self.pid}") launch_service = launch.LaunchService( argv=self.launch_file_arguments, @@ -79,6 +79,51 @@ def stop(self): print(f"Stopping Launch file {self.launch_file_name} from package {self.package_name}") self.kill() +class LaunchManager: + def __init__(self): + self.processes = {} + + def start(self, package_name, launch_file_name): + name = package_name + "/" + launch_file_name + + if name not in self.processes: + try: + self.processes[name] = LaunchProcess(package_name=package_name, launch_file_name=launch_file_name) + self.processes[name].start() + except: + return {"status": "error occured"} + else: + return {"status": "successfuly started"} + else: + return {"status": "already started. ignoring"} + + def status(self, package_name, launch_file_name): + name = package_name + "/" + launch_file_name + + if name in self.processes: + if self.processes[name].is_alive(): + return {"status": "running"} + else: + return {"status": "failed"} + else: + return {"status": "stopped"} + + + def stop(self, package_name, launch_file_name): + name = package_name + "/" + launch_file_name + + if name in self.processes: + try: + self.processes[name].stop() + self.processes[name].join() + except: + return {"status": "error occured"} + else: + return {"status": "successfuly stopped"} + else: + return {"status": "already stopped. ignoring"} + + if __name__ == "__main__": launch_process = LaunchProcess( package_name="nav_stack", diff --git a/capabilities2_launch/launch_server/server.py b/capabilities2_launch/launch_server/server.py new file mode 100644 index 0000000..beb0251 --- /dev/null +++ b/capabilities2_launch/launch_server/server.py @@ -0,0 +1,65 @@ +from launch_server.launch_process import LaunchManager +from multiprocessing.connection import Listener + +class LaunchServer: + def __init__(self): + self.address = ('localhost', 6000) + self.listener = Listener(self.address, authkey=b'capabilities2') + self.manager = LaunchManager() + + def initialize(self): + self.connection = self.listener.accept() + print("[Launch Server] connection Accepted from {self.listener.last_accepted}") + + def run(self): + while True: + # message would be a dictionary with the format of {"command": , "package": , "launch_file": } + message_incoming = self.connection.recv() + + command = message_incoming["command"] + + if ((command=="start") or (command=="stop") or (command=="status")): + package_name = message_incoming["package"] + launch_file_name = message_incoming["launch_file"] + else: + package_name = "" + launch_file_name = "" + + if (command=="start"): + print("[Launch Server] start request for {package_name}/{launch_file_name}") + status = self.manager.start(package_name=package_name, launch_file_name=launch_file_name) + result = status["status"] + + print("[Launch Server] start response of {package_name}/{launch_file_name}: {result}") + + elif (command=="stop"): + print("[Launch Server] stop request for {package_name}/{launch_file_name}") + status = self.manager.stop(package_name=package_name, launch_file_name=launch_file_name) + result = status["status"] + + print("[Launch Server] stop response of {package_name}/{launch_file_name}: {result}") + + elif (command=="status"): + print("[Launch Server] status request for {package_name}/{launch_file_name}") + status = self.manager.status(package_name=package_name, launch_file_name=launch_file_name) + result = status["status"] + + print("[Launch Server] status response of {package_name}/{launch_file_name}: {result}") + + elif (command=="exit"): + break + + else: + print("[Launch Server] does not support command of type : {command}") + break + + self.connection.send(status) + + self.listener.close + + +if __name__ == "__main__": + + server = LaunchServer() + server.initialize() + server.run() \ No newline at end of file diff --git a/capabilities2_launch_py/package.xml b/capabilities2_launch/package.xml similarity index 89% rename from capabilities2_launch_py/package.xml rename to capabilities2_launch/package.xml index 5660d5d..8b0c6fa 100644 --- a/capabilities2_launch_py/package.xml +++ b/capabilities2_launch/package.xml @@ -1,7 +1,7 @@ - capabilities2_launch_py + capabilities2_launch 0.0.0 TODO: Package description kalana @@ -10,6 +10,8 @@ rclcpp capabilities2_msgs + ros2launch + ament_copyright ament_flake8 ament_pep257 diff --git a/capabilities2_launch_py/resource/capabilities2_launch_py b/capabilities2_launch/resource/capabilities2_launch similarity index 100% rename from capabilities2_launch_py/resource/capabilities2_launch_py rename to capabilities2_launch/resource/capabilities2_launch diff --git a/capabilities2_launch/setup.cfg b/capabilities2_launch/setup.cfg new file mode 100644 index 0000000..9844c9c --- /dev/null +++ b/capabilities2_launch/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/capabilities2_launch +[install] +install_scripts=$base/lib/capabilities2_launch diff --git a/capabilities2_launch_py/setup.py b/capabilities2_launch/setup.py similarity index 64% rename from capabilities2_launch_py/setup.py rename to capabilities2_launch/setup.py index 717f564..856147b 100644 --- a/capabilities2_launch_py/setup.py +++ b/capabilities2_launch/setup.py @@ -1,6 +1,8 @@ from setuptools import find_packages, setup +import os +from glob import glob -package_name = 'capabilities2_launch_py' +package_name = 'capabilities2_launch' setup( name=package_name, @@ -9,6 +11,8 @@ data_files=[ ('share/ament_index/resource_index/packages', ['resource/' + package_name]), ('share/' + package_name, ['package.xml']), + (os.path.join('share', package_name, 'launch'), glob(os.path.join('launch', '*launch.[pxy][yma]*'))), + (os.path.join('share', package_name, 'launch_server'), glob(os.path.join('launch_server', '*.py'))), ], install_requires=['setuptools'], zip_safe=True, @@ -19,7 +23,7 @@ tests_require=['pytest'], entry_points={ 'console_scripts': [ - 'capabilities2_launch_py = capabilities2_launch_py.launch_server:main', + 'capabilities2_launch = capabilities2_launch.capabilities2_launch:main', ], }, ) diff --git a/capabilities2_launch_py/test/test_copyright.py b/capabilities2_launch/test/test_copyright.py similarity index 100% rename from capabilities2_launch_py/test/test_copyright.py rename to capabilities2_launch/test/test_copyright.py diff --git a/capabilities2_launch_py/test/test_flake8.py b/capabilities2_launch/test/test_flake8.py similarity index 100% rename from capabilities2_launch_py/test/test_flake8.py rename to capabilities2_launch/test/test_flake8.py diff --git a/capabilities2_launch_py/test/test_pep257.py b/capabilities2_launch/test/test_pep257.py similarity index 100% rename from capabilities2_launch_py/test/test_pep257.py rename to capabilities2_launch/test/test_pep257.py diff --git a/capabilities2_launch_py/capabilities2_launch_py/launch_server.py b/capabilities2_launch_py/capabilities2_launch_py/launch_server.py deleted file mode 100644 index 0f1a462..0000000 --- a/capabilities2_launch_py/capabilities2_launch_py/launch_server.py +++ /dev/null @@ -1,79 +0,0 @@ -import os -import signal -import rclpy -from launch_process import launch_process -from rclpy.node import Node -from capabilities2_msgs.srv import Launch - -class LaunchServer(Node): - def __init__(self): - super().__init__('launch_file_server') - - # Service for starting a launch file - self.start_server = self.create_service( - Launch, - '/capabilities/launch_start', - self.start_request - ) - - # Service for stopping a launch file - self.stop_server = self.create_service( - Launch, - '/capabilities/launch_stop', - self.stop_request - ) - - # Dictionary to track running processes - self.processes = {} - - self.get_logger().info('LaunchFileServer is ready.') - - def start_request(self, request, response): - package_name = request.package_name - launch_name = request.launch_file_name - - self.get_logger().info(f'Received launch start request: package = {package_name}, launch file = {launch_name}') - - name = package_name + "/" + launch_name - - self.processes[name] = launch_process.LaunchProcess(package_name=package_name, launch_file_name=launch_name) - self.processes[name].start() - - return response - - def stop_request(self, request, response): - package_name = request.package_name - launch_name = request.launch_file_name - - self.get_logger().info(f'Received launch stop request: package = {package_name}, launch file = {launch_name}') - - name = package_name + "/" + launch_name - - self.processes[name].stop() - self.processes[name].join() - - return response - - -def main(args=None): - rclpy.init(args=args) - - # Create the LaunchServer node - launch_server = LaunchServer() - - # Keep the node running - try: - rclpy.spin(launch_server) - except KeyboardInterrupt: - launch_server.get_logger().info('Shutting down LaunchFileServer...') - finally: - # Clean up all running processes - for pid, process in launch_server.processes.items(): - os.killpg(os.getpgid(pid), signal.SIGTERM) - process.wait() - - rclpy.shutdown() - - -if __name__ == '__main__': - main() \ No newline at end of file diff --git a/capabilities2_launch_py/setup.cfg b/capabilities2_launch_py/setup.cfg deleted file mode 100644 index 5cc0c4a..0000000 --- a/capabilities2_launch_py/setup.cfg +++ /dev/null @@ -1,4 +0,0 @@ -[develop] -script_dir=$base/lib/capabilities2_launch_py -[install] -install_scripts=$base/lib/capabilities2_launch_py diff --git a/capabilities2_msgs/srv/Launch.srv b/capabilities2_msgs/srv/Launch.srv index b3eedd3..a1938ed 100644 --- a/capabilities2_msgs/srv/Launch.srv +++ b/capabilities2_msgs/srv/Launch.srv @@ -2,4 +2,5 @@ std_msgs/Header header string package_name string launch_file_name --- -std_msgs/Header header \ No newline at end of file +std_msgs/Header header +string status \ No newline at end of file diff --git a/capabilities2_runner/include/capabilities2_runner/action_runner.hpp b/capabilities2_runner/include/capabilities2_runner/action_runner.hpp index 3d6bd35..bcef201 100644 --- a/capabilities2_runner/include/capabilities2_runner/action_runner.hpp +++ b/capabilities2_runner/include/capabilities2_runner/action_runner.hpp @@ -1,6 +1,7 @@ #pragma once #include +#include #include #include #include @@ -82,35 +83,31 @@ class ActionRunner : public RunnerBase { try { - std::shared_future cancel_future = - action_client_->async_cancel_goal( - goal_handle_, [this](action_msgs::srv::CancelGoal_Response::SharedPtr response) { - if (response->return_code != action_msgs::srv::CancelGoal_Response::ERROR_NONE) - { - // throw runner_exception("failed to cancel runner"); - RCLCPP_WARN(node_->get_logger(), "Runner cancelation failed."); - } - - // Trigger on_stopped event if defined - - RCLCPP_INFO(node_->get_logger(), "[%s] on_stopped event available. Triggering", - run_config_.interface.c_str()); - - events[execute_id].on_stopped(update_on_stopped(events[execute_id].on_stopped_param)); - }); + auto cancel_future = action_client_->async_cancel_goal( + goal_handle_, [this](action_msgs::srv::CancelGoal_Response::SharedPtr response) { + if (response->return_code != action_msgs::srv::CancelGoal_Response::ERROR_NONE) + { + // throw runner_exception("failed to cancel runner"); + RCLCPP_WARN(node_->get_logger(), "Runner cancellation failed."); + } + + // Trigger on_stopped event if defined + if (events[execute_id].on_stopped != "") + { + RCLCPP_INFO(node_->get_logger(), "[%s] on_stopped event available. Triggering", + run_config_.interface.c_str()); + triggerFunction_(events[execute_id].on_stopped, update_on_stopped(events[execute_id].on_stopped_param)); + } + }); // wait for action to be stopped. hold the thread for 2 seconds to help keep callbacks in scope // BUG: the line below does not work in jazzy build, so a workaround is used - // rclcpp::spin_until_future_complete(node_->get_node_base_interface(), cancel_future, std::chrono::seconds(2)); auto timeout = std::chrono::steady_clock::now() + std::chrono::seconds(2); while (std::chrono::steady_clock::now() < timeout) { // Check if the cancel operation is complete if (cancel_future.wait_for(std::chrono::milliseconds(100)) == std::future_status::ready) break; - - // Spin some work on the node to keep the callback within scope - rclcpp::spin_some(node_); } } catch (const rclcpp_action::exceptions::UnknownGoalHandleError& e) @@ -122,102 +119,109 @@ class ActionRunner : public RunnerBase } /** - * @brief the trigger function on the action runner is used to trigger an action. - * this method provides a mechanism for injecting parameters or a goal into the action - * and then trigger the action + * @brief Trigger process to be executed. * - * @param parameters - * @return std::optional> + * This method utilizes paramters set via the trigger() function + * + * @param parameters pointer to tinyxml2::XMLElement that contains parameters */ - virtual std::optional> - trigger(tinyxml2::XMLElement* parameters = nullptr) override + virtual void execution(int id) override { execute_id += 1; // if parameters are not provided then cannot proceed - if (!parameters) + if (!parameters_[id]) throw runner_exception("cannot trigger action without parameters"); // generate a goal from parameters if provided - typename ActionT::Goal goal_msg = generate_goal(parameters); + goal_msg_ = generate_goal(parameters_[id]); + + RCLCPP_INFO(node_->get_logger(), "[%s/%d] goal generated.", run_config_.interface.c_str(), id); - RCLCPP_INFO(node_->get_logger(), "[%s] goal generated.", run_config_.interface.c_str()); + std::unique_lock lock(send_goal_mutex); + action_complete = false; - // send goal options - // goal response callback + // trigger the action client with goal send_goal_options_.goal_response_callback = - [this](const typename rclcpp_action::ClientGoalHandle::SharedPtr& goal_handle) { + [this, id](const typename rclcpp_action::ClientGoalHandle::SharedPtr& goal_handle) { if (goal_handle) { - RCLCPP_INFO(node_->get_logger(), "[%s] on_started event available. Triggering", - run_config_.interface.c_str()); - events[execute_id].on_started(update_on_started(events[execute_id].on_started_param)); + RCLCPP_INFO(node_->get_logger(), "[%s/%d] goal accepted. Waiting for result", run_config_.interface.c_str(), + id); + + // trigger the events related to on_started state + if (events[execute_id].on_started != "") + { + RCLCPP_INFO(node_->get_logger(), "[%s/%d] on_started event available. Triggering", + run_config_.interface.c_str(), id); + + triggerFunction_(events[execute_id].on_started, update_on_started(events[execute_id].on_started_param)); + } } else { - RCLCPP_ERROR(node_->get_logger(), "[%s] goal rejected", run_config_.interface.c_str()); + RCLCPP_ERROR(node_->get_logger(), "[%s/%d] goal rejected", run_config_.interface.c_str(), id); } // store goal handle to be used with stop funtion goal_handle_ = goal_handle; }; - // result callback + send_goal_options_.feedback_callback = + [this, id](typename rclcpp_action::ClientGoalHandle::SharedPtr goal_handle, + const typename ActionT::Feedback::ConstSharedPtr feedback_msg) { + std::string feedback = generate_feedback(feedback_msg); + + if (feedback != "") + { + RCLCPP_INFO(node_->get_logger(), "[%s/%d] received feedback: %s", run_config_.interface.c_str(), id, + feedback.c_str()); + } + }; + send_goal_options_.result_callback = - [this](const typename rclcpp_action::ClientGoalHandle::WrappedResult& wrapped_result) { + [this, id](const typename rclcpp_action::ClientGoalHandle::WrappedResult& wrapped_result) { + RCLCPP_INFO(node_->get_logger(), "[%s/%d] received result", run_config_.interface.c_str(), id); + if (wrapped_result.code == rclcpp_action::ResultCode::SUCCEEDED) { - // Trigger on_success event if defined - RCLCPP_INFO(node_->get_logger(), "[%s] on_success event available. Triggering", - run_config_.interface.c_str()); - events[execute_id].on_success(update_on_success(events[execute_id].on_success_param)); + RCLCPP_INFO(node_->get_logger(), "[%s/%d] action succeeded.", run_config_.interface.c_str(), id); + + // trigger the events related to on_success state + if (events[execute_id].on_success != "") + { + RCLCPP_INFO(node_->get_logger(), "[%s/%d] on_success event available. Triggering", + run_config_.interface.c_str(), id); + triggerFunction_(events[execute_id].on_success, update_on_success(events[execute_id].on_success_param)); + } } else { - // Trigger on_failure event if defined - RCLCPP_INFO(node_->get_logger(), "[%s] on_failure event available. Triggering", - run_config_.interface.c_str()); - - events[execute_id].on_failure(update_on_failure(events[execute_id].on_failure_param)); + RCLCPP_ERROR(node_->get_logger(), "[%s/%d] action failed.", run_config_.interface.c_str(), id); + RCLCPP_ERROR(node_->get_logger(), "[%s/%d] error code: %d", run_config_.interface.c_str(), id, + static_cast(wrapped_result.code)); + + // trigger the events related to on_failure state + if (events[execute_id].on_failure != "") + { + RCLCPP_INFO(node_->get_logger(), "[%s/%d] on_failure event available. Triggering", + run_config_.interface.c_str(), id); + triggerFunction_(events[execute_id].on_failure, update_on_failure(events[execute_id].on_failure_param)); + } } result_ = wrapped_result.result; + action_complete = true; + send_goal_cv.notify_all(); }; - // trigger the action client with goal - auto goal_handle_future = action_client_->async_send_goal(goal_msg, send_goal_options_); + goal_handle_future_ = action_client_->async_send_goal(goal_msg_, send_goal_options_); - RCLCPP_INFO(node_->get_logger(), "[%s] goal sent. Waiting for acceptance", run_config_.interface.c_str()); + RCLCPP_INFO(node_->get_logger(), "[%s/%d] goal sent. Waiting for acceptance", run_config_.interface.c_str(), id); - // create a function to call for the result. the future will be returned to the caller and the caller - // can provide a conversion function to handle the result - - std::function result_callback = [this, - goal_handle_future](tinyxml2::XMLElement* result) { - auto goal_handle = goal_handle_future.get(); - - RCLCPP_INFO(node_->get_logger(), "[%s] goal accepted. Waiting for result", run_config_.interface.c_str()); - - if (!goal_handle) - { - RCLCPP_INFO(node_->get_logger(), "[%s] goal rejected", run_config_.interface.c_str()); - return; - } - - // Get the result asynchronously - auto result_future = action_client_->async_get_result(goal_handle); - - // Wait for result future - auto wrapped_result = result_future.get(); - - // convert the result - if (wrapped_result.code == rclcpp_action::ResultCode::SUCCEEDED) - { - result = generate_result(wrapped_result.result); - } - }; - - return result_callback; + // Conditional wait + send_goal_cv.wait(lock, [this] { return action_complete; }); + RCLCPP_INFO(node_->get_logger(), "[%s/%d] action complete. Thread closing.", run_config_.interface.c_str(), id); } protected: @@ -235,17 +239,17 @@ class ActionRunner : public RunnerBase virtual typename ActionT::Goal generate_goal(tinyxml2::XMLElement* parameters) = 0; /** - * @brief generate a typed erased goal result + * @brief Generate a std::string from feedback message * - * this method is used in a callback passed to the trigger caller to get type erased result - * from the action the result can be passed by the caller or ignored + * This function is used to convert feedback messages into generic strings * - * The pattern needs to be implemented in the derived class + * A pattern needs to be implemented in the derived class. If the feedback string + * is empty, nothing will be printed on the screen * - * @param wrapped_result - * @return tinyxml2::XMLElement* + * @param parameters + * @return ActionT::Feedback the received feedback */ - virtual tinyxml2::XMLElement* generate_result(const typename ActionT::Result::SharedPtr& result) = 0; + virtual std::string generate_feedback(const typename ActionT::Feedback::ConstSharedPtr msg) = 0; /**< action client */ typename rclcpp_action::Client::SharedPtr action_client_; @@ -257,8 +261,25 @@ class ActionRunner : public RunnerBase /** goal handle parameter to capture goal response from goal_response_callback */ typename rclcpp_action::ClientGoalHandle::SharedPtr goal_handle_; + /** Wrapped Result */ + typename rclcpp_action::ClientGoalHandle::WrappedResult wrapped_result_; + /** Result */ typename ActionT::Result::SharedPtr result_; + + /** Goal message */ + typename ActionT::Goal goal_msg_; + + /** Goal Handle Future message */ + std::shared_future::SharedPtr> goal_handle_future_; + + /** Result Future*/ + std::shared_future::WrappedResult> result_future_; + + // Synchronization tools + std::mutex mutex_; + std::condition_variable cv_; + bool action_completed_; }; } // namespace capabilities2_runner diff --git a/capabilities2_runner/include/capabilities2_runner/launch_runner.hpp b/capabilities2_runner/include/capabilities2_runner/launch_runner.hpp index 82b32f4..2697bf3 100644 --- a/capabilities2_runner/include/capabilities2_runner/launch_runner.hpp +++ b/capabilities2_runner/include/capabilities2_runner/launch_runner.hpp @@ -37,36 +37,36 @@ class LaunchRunner : public RunnerBase launch_name = run_config_.runner.substr(run_config_.runner.find("/") + 1); // create an service client - start_service_client_ = node_->create_client("/capabilities/launch_start"); + start_service_client_ = node_->create_client("/capabilities/launch/start"); - RCLCPP_INFO(node_->get_logger(), "%s waiting for service: /capabilities/launch_start", + RCLCPP_INFO(node_->get_logger(), "%s waiting for service: /capabilities/launch/start", run_config_.interface.c_str()); if (!start_service_client_->wait_for_service(std::chrono::seconds(3))) { - RCLCPP_ERROR(node_->get_logger(), "%s failed to connect to service: /capabilities/launch_start", + RCLCPP_ERROR(node_->get_logger(), "%s failed to connect to service: /capabilities/launch/start", run_config_.interface.c_str()); - throw runner_exception("Failed to connect to server: /capabilities/launch_start"); + throw runner_exception("Failed to connect to server: /capabilities/launch/start"); } - RCLCPP_INFO(node_->get_logger(), "%s connected to service: /capabilities/launch_start", + RCLCPP_INFO(node_->get_logger(), "%s connected to service: /capabilities/launch/start", run_config_.interface.c_str()); // create an service client - stop_service_client_ = node_->create_client("/capabilities/launch_stop"); + stop_service_client_ = node_->create_client("/capabilities/launch/stop"); // wait for action server - RCLCPP_INFO(node_->get_logger(), "%s waiting for service: /capabilities/launch_stop", + RCLCPP_INFO(node_->get_logger(), "%s waiting for service: /capabilities/launch/stop", run_config_.interface.c_str()); if (!stop_service_client_->wait_for_service(std::chrono::seconds(3))) { - RCLCPP_ERROR(node_->get_logger(), "%s failed to connect to service: /capabilities/launch_stop", + RCLCPP_ERROR(node_->get_logger(), "%s failed to connect to service: /capabilities/launch/stop", run_config_.interface.c_str()); - throw runner_exception("Failed to connect to server: /capabilities/launch_stop"); + throw runner_exception("Failed to connect to server: /capabilities/launch/stop"); } - RCLCPP_INFO(node_->get_logger(), "%s connected to service: /capabilities/launch_stop", + RCLCPP_INFO(node_->get_logger(), "%s connected to service: /capabilities/launch/stop", run_config_.interface.c_str()); // generate a reequest from launch_name and package_name @@ -122,11 +122,18 @@ class LaunchRunner : public RunnerBase } // throw on trigger function - std::optional> trigger(tinyxml2::XMLElement* parameters) override + void trigger(const std::string& parameters) override { throw runner_exception("No Trigger as this is launch runner"); } +protected: + // throw on triggerExecution function + void execution(int id) override + { + throw runner_exception("no triggerExecution() this is a no-trigger action runner"); + } + std::string launch_name; std::string package_name; diff --git a/capabilities2_runner/include/capabilities2_runner/notrigger_action_runner.hpp b/capabilities2_runner/include/capabilities2_runner/notrigger_action_runner.hpp index 0dc8a8b..1287105 100644 --- a/capabilities2_runner/include/capabilities2_runner/notrigger_action_runner.hpp +++ b/capabilities2_runner/include/capabilities2_runner/notrigger_action_runner.hpp @@ -18,22 +18,23 @@ class NoTriggerActionRunner : public ActionRunner { public: // throw on trigger function - std::optional> - trigger(tinyxml2::XMLElement* parameters) override + void trigger(tinyxml2::XMLElement* parameters) override { throw runner_exception("cannot trigger this is a no-trigger action runner"); } protected: - // throw on xml conversion functions - typename ActionT::Goal generate_goal(tinyxml2::XMLElement*) override + + // throw on triggerExecution function + void execution() override { - throw runner_exception("cannot generate goal this is a no-trigger action runner"); + throw runner_exception("no triggerExecution() this is a no-trigger action runner"); } - tinyxml2::XMLElement* generate_result(const typename ActionT::Result::SharedPtr& result) override + // throw on xml conversion functions + typename ActionT::Goal generate_goal(tinyxml2::XMLElement*) override { - throw runner_exception("cannot generate result this is a no-trigger action runner"); + throw runner_exception("cannot generate goal this is a no-trigger action runner"); } }; diff --git a/capabilities2_runner/include/capabilities2_runner/runner_base.hpp b/capabilities2_runner/include/capabilities2_runner/runner_base.hpp index 4027760..2733b44 100644 --- a/capabilities2_runner/include/capabilities2_runner/runner_base.hpp +++ b/capabilities2_runner/include/capabilities2_runner/runner_base.hpp @@ -3,7 +3,8 @@ #include #include #include -#include +#include +#include #include #include @@ -63,25 +64,25 @@ struct runner_opts * * keeps track of events that are related to runner instances at various points of the * plan - * @param on_started pointer to function to execute on start - * @param on_success pointer to function to execute on success - * @param on_failure pointer to function to execute on failure - * @param on_stopped pointer to function to execute on stop - * @param on_started_param parameters for the function to execute on start - * @param on_success_param parameters for the function to execute on success - * @param on_failure_param parameters for the function to execute on failure - * @param on_stopped_param parameters for the function to execute on stop + * @param on_started name of the capability to execute on start + * @param on_success name of the capability to execute on success + * @param on_failure name of the capability to execute on failure + * @param on_stopped name of the capability to execute on stop + * @param on_started_param parameters for the capability to execute on start + * @param on_success_param parameters for the capability to execute on success + * @param on_failure_param parameters for the capability to execute on failure + * @param on_stopped_param parameters for the capability to execute on stop */ struct event_opts { - std::function on_started; - std::function on_success; - std::function on_failure; - std::function on_stopped; - tinyxml2::XMLElement* on_started_param; - tinyxml2::XMLElement* on_success_param; - tinyxml2::XMLElement* on_failure_param; - tinyxml2::XMLElement* on_stopped_param; + std::string on_started; + std::string on_success; + std::string on_failure; + std::string on_stopped; + std::string on_started_param; + std::string on_success_param; + std::string on_failure_param; + std::string on_stopped_param; }; class RunnerBase @@ -112,18 +113,26 @@ class RunnerBase virtual void stop() = 0; /** - * @brief trigger the runner + * @brief Trigger the runner * - * this method allows insertion of parameters in a runner after it has been initialized - * it is an approach to parameterise capabilities + * This method allows insertion of parameters in a runner after it has been initialized. it is an approach + * to parameterise capabilities. Internally starts up RunnerBase::triggerExecution in a thread * * @param parameters pointer to tinyxml2::XMLElement that contains parameters * - * @return std::optional)>> function pointer to invoke - * elsewhere such as an event callback */ - virtual std::optional> - trigger(tinyxml2::XMLElement* parameters = nullptr) = 0; + virtual void trigger(const std::string& parameters) + { + RCLCPP_INFO(node_->get_logger(), "[%s/%d] received new parameters", run_config_.interface.c_str(), 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); + + thread_id += 1; + } /** * @brief Initializer function for initializing the base runner in place of constructor due to plugin semantics @@ -138,19 +147,24 @@ class RunnerBase run_config_ = run_config; insert_id = 0; execute_id = -1; + thread_id = 0; } /** * @brief attach events to the runner * * @param event_option event_options related for the action - * + * * @return number of attached events */ - int attach_events(capabilities2_runner::event_opts& event_option) + int attach_events(capabilities2_runner::event_opts& event_option, + std::function triggerFunction) { - RCLCPP_INFO(node_->get_logger(), "%s accepted event options with ID : %d ", run_config_.interface.c_str(), insert_id); - + RCLCPP_INFO(node_->get_logger(), "[%s] accepted event options with ID : %d ", run_config_.interface.c_str(), + insert_id); + + triggerFunction_ = triggerFunction; + events[insert_id] = event_option; insert_id += 1; @@ -198,6 +212,16 @@ class RunnerBase } protected: + /** + * @brief Trigger process to be executed. + * + * This method utilizes paramters set via the trigger() function + * + * @param parameters pointer to tinyxml2::XMLElement that contains parameters + * + */ + virtual void execution(int id) = 0; + /** * @brief Update on_started event parameters with new data if avaible. * @@ -209,7 +233,7 @@ class RunnerBase * @param parameters pointer to the XMLElement containing parameters * @return pointer to the XMLElement containing updated parameters */ - virtual tinyxml2::XMLElement* update_on_started(tinyxml2::XMLElement* parameters) + virtual std::string update_on_started(std::string& parameters) { return parameters; }; @@ -225,7 +249,7 @@ class RunnerBase * @param parameters pointer to the XMLElement containing parameters * @return pointer to the XMLElement containing updated parameters */ - virtual tinyxml2::XMLElement* update_on_stopped(tinyxml2::XMLElement* parameters) + virtual std::string update_on_stopped(std::string& parameters) { return parameters; }; @@ -241,7 +265,7 @@ class RunnerBase * @param parameters pointer to the XMLElement containing parameters * @return pointer to the XMLElement containing updated parameters */ - virtual tinyxml2::XMLElement* update_on_failure(tinyxml2::XMLElement* parameters) + virtual std::string update_on_failure(std::string& parameters) { return parameters; }; @@ -257,11 +281,55 @@ class RunnerBase * @param parameters pointer to the XMLElement containing parameters * @return pointer to the XMLElement containing updated parameters */ - virtual tinyxml2::XMLElement* update_on_success(tinyxml2::XMLElement* parameters) + virtual std::string update_on_success(std::string& parameters) { return parameters; }; + /** + * @brief convert an XMLElement to std::string + * + * @param element XMLElement element to be converted + * @param paramters parameter to hold std::string + * + * @return `true` if element is not nullptr and conversion successful, `false` if element is nullptr + */ + std::string convert_to_string(tinyxml2::XMLElement* element) + { + if (element) + { + element->Accept(&printer); + std::string parameters = printer.CStr(); + return parameters; + } + else + { + std::string parameters = ""; + return parameters; + } + } + + /** + * @brief convert an XMLElement to std::string + * + * @param element XMLElement element to be converted + * @param paramters parameter to hold std::string + * + * @return `true` if element is not nullptr and conversion successful, `false` if element is nullptr + */ + tinyxml2::XMLElement* convert_to_xml(const std::string& parameters) + { + if (parameters != "") + { + doc.Parse(parameters.c_str()); + tinyxml2::XMLElement* element = doc.FirstChildElement(); + return element; + } + else + { + return nullptr; + } + } // run config getters /** @@ -412,19 +480,55 @@ class RunnerBase std::map events; /** - * @brief Last tracker id to be inserted + * @brief Last event tracker id to be inserted */ int insert_id; /** - * @brief Last tracker id to be executed + * @brief Last parameter tracker id to be executed */ int execute_id; + /** + * @brief Last parameter tracker id to be executed + */ + int thread_id; + /** * @brief pointer to XMLElement which contain parameters - * */ - tinyxml2::XMLElement* parameters_; + */ + std::map parameters_; + + /** + * @brief dictionary of threads that executes the triggerExecution functionality + */ + std::map executionThreadPool; + + /** + * @brief mutex and conditional variable for threadpool synchronisation. + */ + std::mutex send_goal_mutex; + std::condition_variable send_goal_cv; + + /** + * @brief boolean flag for thread completion. + */ + bool action_complete; + + /** + * @brief external function that triggers capability runners + */ + std::function triggerFunction_; + + /** + * @brief XMLElement that is used to convert xml strings to std::string + */ + tinyxml2::XMLPrinter printer; + + /** + * @brief XMLElement that is used to convert std::string to xml strings + */ + tinyxml2::XMLDocument doc; }; } // namespace capabilities2_runner diff --git a/capabilities2_runner/include/capabilities2_runner/service_runner.hpp b/capabilities2_runner/include/capabilities2_runner/service_runner.hpp index 73201d8..6fbf2d2 100644 --- a/capabilities2_runner/include/capabilities2_runner/service_runner.hpp +++ b/capabilities2_runner/include/capabilities2_runner/service_runner.hpp @@ -52,61 +52,54 @@ class ServiceRunner : public RunnerBase } /** - * @brief the trigger function on the service runner is used to trigger an service. - * this method provides a mechanism for injecting parameters or a goal into a service - * and then trigger the service + * @brief Trigger process to be executed. * - * @param parameters - * @return std::optional> + * This method utilizes paramters set via the trigger() function + * + * @param parameters pointer to tinyxml2::XMLElement that contains parameters */ - virtual std::optional> - trigger(tinyxml2::XMLElement* parameters = nullptr) override + virtual void execution(int id) override { execute_id += 1; // if parameters are not provided then cannot proceed - if (!parameters) + if (!parameters_[id]) throw runner_exception("cannot trigger service without parameters"); // generate a goal from parameters if provided - auto request_msg = std::make_shared(generate_request(parameters)); + auto request_msg = std::make_shared(generate_request(parameters_[id])); auto result_future = service_client_->async_send_request( request_msg, [this](typename rclcpp::Client::SharedFuture future) { if (!future.valid()) { RCLCPP_ERROR(node_->get_logger(), "get result call failed"); - events[execute_id].on_failure(update_on_failure(events[execute_id].on_failure_param)); + + // trigger the events related to on_failure state + if (events[execute_id].on_failure != "") + { + triggerFunction_(events[execute_id].on_failure, update_on_failure(events[execute_id].on_failure_param)); + } } else { RCLCPP_INFO(node_->get_logger(), "get result call succeeded"); - events[execute_id].on_success(update_on_success(events[execute_id].on_success_param)); - } - }); - - // Trigger started event if defined - events[execute_id].on_started(update_on_started(events[execute_id].on_started_param)); - // Define a callback function to handle the result once it's ready - std::function result_callback = - [this, &result_future](tinyxml2::XMLElement* result) mutable { - auto response = result_future.get(); - result = generate_response(response); + response_ = future.get(); - // Ensure the future is ready before accessing the result - if (result_future.wait_for(std::chrono::seconds(0)) == std::future_status::ready) - { - auto response = result_future.get(); - result = generate_response(response); + // trigger the events related to on_success state + if (events[execute_id].on_success != "") + { + triggerFunction_(events[execute_id].on_success, update_on_success(events[execute_id].on_success_param)); + } } - else - { - RCLCPP_WARN(node_->get_logger(), "Result is not ready yet."); - } - }; + }); - return result_callback; + // trigger the events related to on_started state + if (events[execute_id].on_started != "") + { + triggerFunction_(events[execute_id].on_started, update_on_started(events[execute_id].on_started_param)); + } } /** @@ -127,8 +120,11 @@ class ServiceRunner : public RunnerBase if (!service_client_) throw runner_exception("cannot stop runner action that was not started"); - // publish event - events[execute_id].on_stopped(update_on_stopped(events[execute_id].on_stopped_param)); + // Trigger on_stopped event if defined + if (events[execute_id].on_stopped != "") + { + triggerFunction_(events[execute_id].on_stopped, update_on_stopped(events[execute_id].on_stopped_param)); + } } protected: @@ -145,19 +141,7 @@ class ServiceRunner : public RunnerBase */ virtual typename ServiceT::Request generate_request(tinyxml2::XMLElement* parameters) = 0; - /** - * @brief generate a typed erased response - * - * This method is used in a callback passed to the trigger caller to get type erased result - * from the service the reponse can be passed by the caller or ignored - * - * The pattern needs to be implemented in the derived class - * - * @param wrapped_result - * @return tinyxml2::XMLElement* - */ - virtual tinyxml2::XMLElement* generate_response(const typename ServiceT::Response::SharedPtr& result) const = 0; - typename rclcpp::Client::SharedPtr service_client_; + typename ServiceT::Response::SharedPtr response_; }; } // namespace capabilities2_runner \ No newline at end of file diff --git a/capabilities2_runner/include/capabilities2_runner/topic_runner.hpp b/capabilities2_runner/include/capabilities2_runner/topic_runner.hpp index 266d22c..2450851 100644 --- a/capabilities2_runner/include/capabilities2_runner/topic_runner.hpp +++ b/capabilities2_runner/include/capabilities2_runner/topic_runner.hpp @@ -44,41 +44,44 @@ class TopicRunner : public RunnerBase } /** - * @brief the trigger function on the topic subscriber runner is used to trigger data retrival. + * @brief Trigger process to be executed. * - * @param parameters - * @return std::optional> + * This method utilizes paramters set via the trigger() function + * + * @param parameters pointer to tinyxml2::XMLElement that contains parameters */ - virtual std::optional> - trigger(tinyxml2::XMLElement* parameters = nullptr) override + virtual void execution(int id) override { execute_id += 1; // if parameters are not provided then cannot proceed - if (!parameters) + if (!parameters_[id]) throw runner_exception("cannot grab data without parameters"); - events[execute_id].on_started(update_on_started(events[execute_id].on_started_param)); + // trigger the events related to on_started state + if (events[execute_id].on_started != "") + { + triggerFunction_(events[execute_id].on_started, update_on_started(events[execute_id].on_started_param)); + } if (latest_message_) { - events[execute_id].on_success(update_on_success(events[execute_id].on_success_param)); + // trigger the events related to on_success state + if (events[execute_id].on_success != "") + { + triggerFunction_(events[execute_id].on_success, update_on_success(events[execute_id].on_success_param)); + } } else { RCLCPP_ERROR(node_->get_logger(), "get result call failed"); - events[execute_id].on_failure(update_on_failure(events[execute_id].on_failure_param)); - } - // create a function to call for the result. the future will be returned to the caller and the caller - // can provide a conversion function to handle the result - - std::function result_callback = [this](tinyxml2::XMLElement* result) { - if (latest_message_) - result = generate_message(latest_message_); - }; - - return result_callback; + // trigger the events related to on_failure state + if (events[execute_id].on_failure != "") + { + triggerFunction_(events[execute_id].on_failure, update_on_failure(events[execute_id].on_failure_param)); + } + } } /** @@ -99,8 +102,11 @@ class TopicRunner : public RunnerBase if (!subscription_) throw runner_exception("cannot stop runner subscriber that was not started"); - // publish event - events[execute_id].on_stopped(update_on_stopped(events[execute_id].on_stopped_param)); + // Trigger on_stopped event if defined + if (events[execute_id].on_stopped != "") + { + triggerFunction_(events[execute_id].on_stopped, update_on_stopped(events[execute_id].on_stopped_param)); + } } protected: @@ -117,19 +123,6 @@ class TopicRunner : public RunnerBase latest_message_ = msg; } - /** - * @brief generate xml data structure of the message - * - * this method is used in a callback passed to the trigger caller to get type erased result - * of the latest message can be passed by the caller or ignored - * - * The pattern needs to be implemented in the derived class - * - * @param wrapped_result - * @return tinyxml2::XMLElement* - */ - virtual tinyxml2::XMLElement* generate_message(typename TopicT::SharedPtr& result) = 0; - typename rclcpp::Subscription::SharedPtr subscription_; mutable typename TopicT::SharedPtr latest_message_{ nullptr }; diff --git a/capabilities2_runner/src/standard_runners.cpp b/capabilities2_runner/src/standard_runners.cpp index 88924a5..58ca354 100644 --- a/capabilities2_runner/src/standard_runners.cpp +++ b/capabilities2_runner/src/standard_runners.cpp @@ -26,11 +26,16 @@ class DummyRunner : public RunnerBase RCLCPP_INFO(node_->get_logger(), "Dummy runner stopped"); } - std::optional> - trigger(tinyxml2::XMLElement* parameters) override + void trigger(const std::string& parameters) override { RCLCPP_INFO(node_->get_logger(), "Dummy runner cannot trigger"); - return std::nullopt; + } + +protected: + // throw on triggerExecution function + void execution(int id) override + { + RCLCPP_INFO(node_->get_logger(), "Dummy runner does not have triggerExecution()"); } }; diff --git a/capabilities2_runner_audio/include/capabilities2_runner_audio/listen_runner.hpp b/capabilities2_runner_audio/include/capabilities2_runner_audio/listen_runner.hpp index f507939..84fd7a7 100644 --- a/capabilities2_runner_audio/include/capabilities2_runner_audio/listen_runner.hpp +++ b/capabilities2_runner_audio/include/capabilities2_runner_audio/listen_runner.hpp @@ -46,8 +46,6 @@ class ListenerRunner : public ActionRunner */ virtual hri_audio_msgs::action::SpeechToText::Goal generate_goal(tinyxml2::XMLElement* parameters) override { - parameters_ = parameters; - hri_audio_msgs::action::SpeechToText::Goal goal_msg; goal_msg.header.stamp = node_->get_clock()->now(); @@ -56,22 +54,16 @@ class ListenerRunner : public ActionRunner } /** - * @brief This generate_result function overrides the generate_result() function from ActionRunner(). Since + * @brief This generate feedback function overrides the generate_feedback() function from ActionRunner() * - * @param result message from SpeechToText action - * @return tinyxml2::XMLElement* in the format '' + * @param msg feedback message from the action server + * @return std::string of feedback information */ - virtual tinyxml2::XMLElement* - generate_result(const hri_audio_msgs::action::SpeechToText::Result::SharedPtr& result) override + virtual std::string + generate_feedback(const typename hri_audio_msgs::action::SpeechToText::Feedback::ConstSharedPtr msg) override { - tinyxml2::XMLDocument doc; - - // Root element - tinyxml2::XMLElement* textElement = doc.NewElement("Text"); - doc.InsertEndChild(textElement); - textElement->SetText(result->stt_text.c_str()); - - return doc.FirstChildElement("Text"); + std::string feedback = ""; + return feedback; } /** @@ -85,15 +77,19 @@ class ListenerRunner : public ActionRunner * @param parameters pointer to the XMLElement containing parameters * @return pointer to the XMLElement containing updated parameters */ - virtual tinyxml2::XMLElement* update_on_success(tinyxml2::XMLElement* parameters) + virtual std::string update_on_success(std::string& parameters) { + tinyxml2::XMLElement* element = convert_to_xml(parameters); + // Create the Pose element as a child of the existing parameters element - tinyxml2::XMLElement* textElement = parameters->GetDocument()->NewElement("Text"); - parameters->InsertEndChild(textElement); + tinyxml2::XMLElement* textElement = element->GetDocument()->NewElement("Text"); + element->InsertEndChild(textElement); textElement->SetText(result_->stt_text.c_str()); - // Return the updated parameters element with Pose added - return parameters; + // Return the updated parameters element with Pose added as string + std::string result = convert_to_string(element); + + return result; }; }; diff --git a/capabilities2_runner_audio/include/capabilities2_runner_audio/speak_runner.hpp b/capabilities2_runner_audio/include/capabilities2_runner_audio/speak_runner.hpp index d8f37bc..014e953 100644 --- a/capabilities2_runner_audio/include/capabilities2_runner_audio/speak_runner.hpp +++ b/capabilities2_runner_audio/include/capabilities2_runner_audio/speak_runner.hpp @@ -45,10 +45,8 @@ class SpeakerRunner : public ActionRunner */ virtual hri_audio_msgs::action::TextToSpeech::Goal generate_goal(tinyxml2::XMLElement* parameters) override { - parameters_ = parameters; - const char **text; - parameters_->QueryStringAttribute("text", text); + parameters->QueryStringAttribute("text", text); std::string tts_text(*text); hri_audio_msgs::action::TextToSpeech::Goal goal_msg; @@ -60,16 +58,16 @@ class SpeakerRunner : public ActionRunner } /** - * @brief This generate_result function overrides the generate_result() function from ActionRunner(). Since this does not - * return a value, return value is nullptr - * - * @param result message from SpeechToText action - * @return nullptr + * @brief This generate feedback function overrides the generate_feedback() function from ActionRunner() + * + * @param msg feedback message from the action server + * @return std::string of feedback information */ - virtual tinyxml2::XMLElement* - generate_result(const hri_audio_msgs::action::TextToSpeech::Result::SharedPtr& result) override + virtual std::string + generate_feedback(const typename hri_audio_msgs::action::TextToSpeech::Feedback::ConstSharedPtr msg) override { - return nullptr; + std::string feedback = ""; + return feedback; } }; diff --git a/capabilities2_runner_bt/include/capabilities2_runner_bt/bt_factory_runner.hpp b/capabilities2_runner_bt/include/capabilities2_runner_bt/bt_factory_runner.hpp index b26ebc0..846f1a8 100644 --- a/capabilities2_runner_bt/include/capabilities2_runner_bt/bt_factory_runner.hpp +++ b/capabilities2_runner_bt/include/capabilities2_runner_bt/bt_factory_runner.hpp @@ -21,13 +21,10 @@ class BTRunnerBase : public RunnerBase, public BT::BehaviorTreeFactory { } - virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config, - std::function on_started = nullptr, - std::function on_terminated = nullptr, - std::function on_stopped = nullptr) + virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config) { // init the runner base - init_base(node, run_config, on_started, on_terminated, on_stopped); + init_base(node, run_config); // register (bt)actions from ROS plugins try @@ -46,42 +43,43 @@ class BTRunnerBase : public RunnerBase, public BT::BehaviorTreeFactory tree_.reset(); } +protected: /** * @brief trigger the behaviour factory with the input data * * @param parameters * @return std::optional)>> */ - virtual std::optional)>> - trigger(std::shared_ptr parameters) + virtual void trigger(const std::string& parameters) override { + tinyxml2::XMLElement* parameters_xml = convert_to_xml(parameters); + // if parameters are not provided then cannot proceed - if (!parameters) + if (!parameters_xml) throw runner_exception("cannot trigger action without parameters"); // create the tree (ptr) - tree_ = std::make_shared(this->createTreeFromText(parameters->GetText())); + tree_ = std::make_shared(this->createTreeFromText(parameters_xml->GetText())); // return the tick function - // the caller can call this function to tick the tree - std::function)> result_callback = - [this](std::shared_ptr result) { - // tick the tree - BT::NodeStatus status = tree_->tickWhileRunning(); + // // the caller can call this function to tick the tree + // std::function)> result_callback = + // [this](std::shared_ptr result) { + // // tick the tree + // BT::NodeStatus status = tree_->tickWhileRunning(); - // fill the result - if (status == BT::NodeStatus::SUCCESS) - result->SetText("OK"); - else - result->SetText("FAIL"); + // // fill the result + // if (status == BT::NodeStatus::SUCCESS) + // result->SetText("OK"); + // else + // result->SetText("FAIL"); - return; - }; + // return; + // }; - return result_callback; + // return result_callback; } -protected: // the tree std::shared_ptr tree_; }; diff --git a/capabilities2_runner_nav2/CMakeLists.txt b/capabilities2_runner_nav2/CMakeLists.txt index 3edafbc..1ce3c58 100644 --- a/capabilities2_runner_nav2/CMakeLists.txt +++ b/capabilities2_runner_nav2/CMakeLists.txt @@ -16,6 +16,7 @@ find_package(rclcpp_action REQUIRED) find_package(pluginlib REQUIRED) find_package(nav2_msgs REQUIRED) find_package(geometry_msgs REQUIRED) +find_package(lifecycle_msgs REQUIRED) find_package(capabilities2_msgs REQUIRED) find_package(capabilities2_runner REQUIRED) @@ -44,6 +45,7 @@ ament_target_dependencies(${PROJECT_NAME} pluginlib nav2_msgs geometry_msgs + lifecycle_msgs capabilities2_msgs capabilities2_runner ) diff --git a/capabilities2_runner_nav2/include/capabilities2_runner_nav2/occupancygrid_runner.hpp b/capabilities2_runner_nav2/include/capabilities2_runner_nav2/occupancygrid_runner.hpp index f5f0ef3..6229a76 100644 --- a/capabilities2_runner_nav2/include/capabilities2_runner_nav2/occupancygrid_runner.hpp +++ b/capabilities2_runner_nav2/include/capabilities2_runner_nav2/occupancygrid_runner.hpp @@ -35,122 +35,6 @@ class OccupancyGridRunner : public TopicRunner } protected: - /** - * @brief This generate goal function overrides the generate_message() function from TopicRunner() - * @param result of the type geometry_msgs::msg::PoseWithCovarianceStamped - - * @return tinyxml2::XMLElement* of the robot's pose - */ - virtual tinyxml2::XMLElement* - generate_message(typename nav_msgs::msg::OccupancyGrid::SharedPtr& result) override - { - tinyxml2::XMLDocument doc; - - // Root element for OccupancyGrid - tinyxml2::XMLElement* occupancyGridElement = doc.NewElement("OccupancyGrid"); - doc.InsertFirstChild(occupancyGridElement); - - // Header element - tinyxml2::XMLElement* headerElement = doc.NewElement("header"); - occupancyGridElement->InsertEndChild(headerElement); - - // Header fields: stamp, frame_id - tinyxml2::XMLElement* stampElement = doc.NewElement("stamp"); - headerElement->InsertEndChild(stampElement); - - tinyxml2::XMLElement* secsElement = doc.NewElement("secs"); - secsElement->SetText(result->header.stamp.sec); - stampElement->InsertEndChild(secsElement); - - tinyxml2::XMLElement* nsecsElement = doc.NewElement("nsecs"); - nsecsElement->SetText(result->header.stamp.nanosec); - stampElement->InsertEndChild(nsecsElement); - - tinyxml2::XMLElement* frameIdElement = doc.NewElement("frame_id"); - frameIdElement->SetText(result->header.frame_id.c_str()); - headerElement->InsertEndChild(frameIdElement); - - // Info element - tinyxml2::XMLElement* infoElement = doc.NewElement("info"); - occupancyGridElement->InsertEndChild(infoElement); - - // Map info fields: resolution, width, height - tinyxml2::XMLElement* resolutionElement = doc.NewElement("resolution"); - resolutionElement->SetText(result->info.resolution); - infoElement->InsertEndChild(resolutionElement); - - tinyxml2::XMLElement* widthElement = doc.NewElement("width"); - widthElement->SetText(result->info.width); - infoElement->InsertEndChild(widthElement); - - tinyxml2::XMLElement* heightElement = doc.NewElement("height"); - heightElement->SetText(result->info.height); - infoElement->InsertEndChild(heightElement); - - // Origin position element - tinyxml2::XMLElement* originElement = doc.NewElement("origin"); - infoElement->InsertEndChild(originElement); - - tinyxml2::XMLElement* positionElement = doc.NewElement("position"); - originElement->InsertEndChild(positionElement); - - tinyxml2::XMLElement* posX = doc.NewElement("x"); - posX->SetText(result->info.origin.position.x); - positionElement->InsertEndChild(posX); - - tinyxml2::XMLElement* posY = doc.NewElement("y"); - posY->SetText(result->info.origin.position.y); - positionElement->InsertEndChild(posY); - - tinyxml2::XMLElement* posZ = doc.NewElement("z"); - posZ->SetText(result->info.origin.position.z); - positionElement->InsertEndChild(posZ); - - // Origin orientation element - tinyxml2::XMLElement* orientationElement = doc.NewElement("orientation"); - originElement->InsertEndChild(orientationElement); - - tinyxml2::XMLElement* oriX = doc.NewElement("x"); - oriX->SetText(result->info.origin.orientation.x); - orientationElement->InsertEndChild(oriX); - - tinyxml2::XMLElement* oriY = doc.NewElement("y"); - oriY->SetText(result->info.origin.orientation.y); - orientationElement->InsertEndChild(oriY); - - tinyxml2::XMLElement* oriZ = doc.NewElement("z"); - oriZ->SetText(result->info.origin.orientation.z); - orientationElement->InsertEndChild(oriZ); - - tinyxml2::XMLElement* oriW = doc.NewElement("w"); - oriW->SetText(result->info.origin.orientation.w); - orientationElement->InsertEndChild(oriW); - - // Map load time - tinyxml2::XMLElement* mapLoadTimeElement = doc.NewElement("map_load_time"); - infoElement->InsertEndChild(mapLoadTimeElement); - - tinyxml2::XMLElement* mapSecsElement = doc.NewElement("secs"); - mapSecsElement->SetText(result->info.map_load_time.sec); - mapLoadTimeElement->InsertEndChild(mapSecsElement); - - tinyxml2::XMLElement* mapNsecsElement = doc.NewElement("nsecs"); - mapNsecsElement->SetText(result->info.map_load_time.nanosec); - mapLoadTimeElement->InsertEndChild(mapNsecsElement); - - // Data element for occupancy grid data - tinyxml2::XMLElement* dataElement = doc.NewElement("data"); - occupancyGridElement->InsertEndChild(dataElement); - - // Convert the occupancy grid data into a space-separated string - std::string data_str; - for (const auto& value : result->data) { - data_str += std::to_string(value) + " "; - } - dataElement->SetText(data_str.c_str()); - - return doc.FirstChildElement("OccupancyGrid"); - } /** * @brief Update on_success event parameters with new data if avaible. @@ -163,99 +47,101 @@ class OccupancyGridRunner : public TopicRunner * @param parameters pointer to the XMLElement containing parameters * @return pointer to the XMLElement containing updated parameters */ - virtual tinyxml2::XMLElement* update_on_success(tinyxml2::XMLElement* parameters) + virtual std::string update_on_success(std::string& parameters) { + tinyxml2::XMLElement* element = convert_to_xml(parameters); + // Create the OccupancyGrid element as a child of the existing parameters element - tinyxml2::XMLElement* occupancyGridElement = parameters->GetDocument()->NewElement("OccupancyGrid"); - parameters->InsertEndChild(occupancyGridElement); + tinyxml2::XMLElement* occupancyGridElement = element->GetDocument()->NewElement("OccupancyGrid"); + element->InsertEndChild(occupancyGridElement); // Header element - tinyxml2::XMLElement* headerElement = parameters->GetDocument()->NewElement("header"); + tinyxml2::XMLElement* headerElement = element->GetDocument()->NewElement("header"); occupancyGridElement->InsertEndChild(headerElement); - tinyxml2::XMLElement* stampElement = parameters->GetDocument()->NewElement("stamp"); + tinyxml2::XMLElement* stampElement = element->GetDocument()->NewElement("stamp"); headerElement->InsertEndChild(stampElement); - tinyxml2::XMLElement* secsElement = parameters->GetDocument()->NewElement("secs"); + tinyxml2::XMLElement* secsElement = element->GetDocument()->NewElement("secs"); secsElement->SetText(latest_message_->header.stamp.sec); stampElement->InsertEndChild(secsElement); - tinyxml2::XMLElement* nsecsElement = parameters->GetDocument()->NewElement("nsecs"); + tinyxml2::XMLElement* nsecsElement = element->GetDocument()->NewElement("nsecs"); nsecsElement->SetText(latest_message_->header.stamp.nanosec); stampElement->InsertEndChild(nsecsElement); - tinyxml2::XMLElement* frameIdElement = parameters->GetDocument()->NewElement("frame_id"); + tinyxml2::XMLElement* frameIdElement = element->GetDocument()->NewElement("frame_id"); frameIdElement->SetText(latest_message_->header.frame_id.c_str()); headerElement->InsertEndChild(frameIdElement); // Info element - tinyxml2::XMLElement* infoElement = parameters->GetDocument()->NewElement("info"); + tinyxml2::XMLElement* infoElement = element->GetDocument()->NewElement("info"); occupancyGridElement->InsertEndChild(infoElement); - tinyxml2::XMLElement* resolutionElement = parameters->GetDocument()->NewElement("resolution"); + tinyxml2::XMLElement* resolutionElement = element->GetDocument()->NewElement("resolution"); resolutionElement->SetText(latest_message_->info.resolution); infoElement->InsertEndChild(resolutionElement); - tinyxml2::XMLElement* widthElement = parameters->GetDocument()->NewElement("width"); + tinyxml2::XMLElement* widthElement = element->GetDocument()->NewElement("width"); widthElement->SetText(latest_message_->info.width); infoElement->InsertEndChild(widthElement); - tinyxml2::XMLElement* heightElement = parameters->GetDocument()->NewElement("height"); + tinyxml2::XMLElement* heightElement = element->GetDocument()->NewElement("height"); heightElement->SetText(latest_message_->info.height); infoElement->InsertEndChild(heightElement); // Origin element - tinyxml2::XMLElement* originElement = parameters->GetDocument()->NewElement("origin"); + tinyxml2::XMLElement* originElement = element->GetDocument()->NewElement("origin"); infoElement->InsertEndChild(originElement); - tinyxml2::XMLElement* positionElement = parameters->GetDocument()->NewElement("position"); + tinyxml2::XMLElement* positionElement = element->GetDocument()->NewElement("position"); originElement->InsertEndChild(positionElement); - tinyxml2::XMLElement* posX = parameters->GetDocument()->NewElement("x"); + tinyxml2::XMLElement* posX = element->GetDocument()->NewElement("x"); posX->SetText(latest_message_->info.origin.position.x); positionElement->InsertEndChild(posX); - tinyxml2::XMLElement* posY = parameters->GetDocument()->NewElement("y"); + tinyxml2::XMLElement* posY = element->GetDocument()->NewElement("y"); posY->SetText(latest_message_->info.origin.position.y); positionElement->InsertEndChild(posY); - tinyxml2::XMLElement* posZ = parameters->GetDocument()->NewElement("z"); + tinyxml2::XMLElement* posZ = element->GetDocument()->NewElement("z"); posZ->SetText(latest_message_->info.origin.position.z); positionElement->InsertEndChild(posZ); - tinyxml2::XMLElement* orientationElement = parameters->GetDocument()->NewElement("orientation"); + tinyxml2::XMLElement* orientationElement = element->GetDocument()->NewElement("orientation"); originElement->InsertEndChild(orientationElement); - tinyxml2::XMLElement* oriX = parameters->GetDocument()->NewElement("x"); + tinyxml2::XMLElement* oriX = element->GetDocument()->NewElement("x"); oriX->SetText(latest_message_->info.origin.orientation.x); orientationElement->InsertEndChild(oriX); - tinyxml2::XMLElement* oriY = parameters->GetDocument()->NewElement("y"); + tinyxml2::XMLElement* oriY = element->GetDocument()->NewElement("y"); oriY->SetText(latest_message_->info.origin.orientation.y); orientationElement->InsertEndChild(oriY); - tinyxml2::XMLElement* oriZ = parameters->GetDocument()->NewElement("z"); + tinyxml2::XMLElement* oriZ = element->GetDocument()->NewElement("z"); oriZ->SetText(latest_message_->info.origin.orientation.z); orientationElement->InsertEndChild(oriZ); - tinyxml2::XMLElement* oriW = parameters->GetDocument()->NewElement("w"); + tinyxml2::XMLElement* oriW = element->GetDocument()->NewElement("w"); oriW->SetText(latest_message_->info.origin.orientation.w); orientationElement->InsertEndChild(oriW); // Map load time - tinyxml2::XMLElement* mapLoadTimeElement = parameters->GetDocument()->NewElement("map_load_time"); + tinyxml2::XMLElement* mapLoadTimeElement = element->GetDocument()->NewElement("map_load_time"); infoElement->InsertEndChild(mapLoadTimeElement); - tinyxml2::XMLElement* mapSecsElement = parameters->GetDocument()->NewElement("secs"); + tinyxml2::XMLElement* mapSecsElement = element->GetDocument()->NewElement("secs"); mapSecsElement->SetText(latest_message_->info.map_load_time.sec); mapLoadTimeElement->InsertEndChild(mapSecsElement); - tinyxml2::XMLElement* mapNsecsElement = parameters->GetDocument()->NewElement("nsecs"); + tinyxml2::XMLElement* mapNsecsElement = element->GetDocument()->NewElement("nsecs"); mapNsecsElement->SetText(latest_message_->info.map_load_time.nanosec); mapLoadTimeElement->InsertEndChild(mapNsecsElement); // Data element for occupancy data (converted to a string) - tinyxml2::XMLElement* dataElement = parameters->GetDocument()->NewElement("data"); + tinyxml2::XMLElement* dataElement = element->GetDocument()->NewElement("data"); occupancyGridElement->InsertEndChild(dataElement); std::string data_str; @@ -265,7 +151,9 @@ class OccupancyGridRunner : public TopicRunner dataElement->SetText(data_str.c_str()); // Return the updated parameters element with OccupancyGrid added - return parameters; + std::string result = convert_to_string(element); + + return result; }; }; } // namespace capabilities2_runner \ No newline at end of file diff --git a/capabilities2_runner_nav2/include/capabilities2_runner_nav2/robotpose_runner.hpp b/capabilities2_runner_nav2/include/capabilities2_runner_nav2/robotpose_runner.hpp index 83904d7..763d5a5 100644 --- a/capabilities2_runner_nav2/include/capabilities2_runner_nav2/robotpose_runner.hpp +++ b/capabilities2_runner_nav2/include/capabilities2_runner_nav2/robotpose_runner.hpp @@ -35,62 +35,6 @@ class RobotPoseRunner : public TopicRunnerInsertEndChild(positionElement); - - // Position x, y, z - tinyxml2::XMLElement* posX = doc.NewElement("x"); - posX->SetText(result->pose.pose.position.x); // Set x position value - positionElement->InsertEndChild(posX); - - tinyxml2::XMLElement* posY = doc.NewElement("y"); - posY->SetText(result->pose.pose.position.y); // Set y position value - positionElement->InsertEndChild(posY); - - tinyxml2::XMLElement* posZ = doc.NewElement("z"); - posZ->SetText(result->pose.pose.position.z); // Set z position value - positionElement->InsertEndChild(posZ); - - // Orientation element - tinyxml2::XMLElement* orientationElement = doc.NewElement("orientation"); - poseElement->InsertEndChild(orientationElement); - - // Orientation x, y, z, w - tinyxml2::XMLElement* oriX = doc.NewElement("x"); - oriX->SetText(result->pose.pose.orientation.x); // Set orientation x value - orientationElement->InsertEndChild(oriX); - - tinyxml2::XMLElement* oriY = doc.NewElement("y"); - oriY->SetText(result->pose.pose.orientation.y); // Set orientation y value - orientationElement->InsertEndChild(oriY); - - tinyxml2::XMLElement* oriZ = doc.NewElement("z"); - oriZ->SetText(result->pose.pose.orientation.z); // Set orientation z value - orientationElement->InsertEndChild(oriZ); - - tinyxml2::XMLElement* oriW = doc.NewElement("w"); - oriW->SetText(result->pose.pose.orientation.w); // Set orientation w value - orientationElement->InsertEndChild(oriW); - - return doc.FirstChildElement("Pose"); - } - /** * @brief Update on_success event parameters with new data if avaible. * @@ -102,52 +46,56 @@ class RobotPoseRunner : public TopicRunnerGetDocument()->NewElement("Pose"); - parameters->InsertEndChild(poseElement); + tinyxml2::XMLElement* poseElement = element->GetDocument()->NewElement("Pose"); + element->InsertEndChild(poseElement); // Position element - tinyxml2::XMLElement* positionElement = parameters->GetDocument()->NewElement("position"); + tinyxml2::XMLElement* positionElement = element->GetDocument()->NewElement("position"); poseElement->InsertEndChild(positionElement); // Position x, y, z - tinyxml2::XMLElement* posX = parameters->GetDocument()->NewElement("x"); + tinyxml2::XMLElement* posX = element->GetDocument()->NewElement("x"); posX->SetText(latest_message_->pose.pose.position.x); // Set x position value positionElement->InsertEndChild(posX); - tinyxml2::XMLElement* posY = parameters->GetDocument()->NewElement("y"); + tinyxml2::XMLElement* posY = element->GetDocument()->NewElement("y"); posY->SetText(latest_message_->pose.pose.position.y); // Set y position value positionElement->InsertEndChild(posY); - tinyxml2::XMLElement* posZ = parameters->GetDocument()->NewElement("z"); + tinyxml2::XMLElement* posZ = element->GetDocument()->NewElement("z"); posZ->SetText(latest_message_->pose.pose.position.z); // Set z position value positionElement->InsertEndChild(posZ); // Orientation element - tinyxml2::XMLElement* orientationElement = parameters->GetDocument()->NewElement("orientation"); + tinyxml2::XMLElement* orientationElement = element->GetDocument()->NewElement("orientation"); poseElement->InsertEndChild(orientationElement); // Orientation x, y, z, w - tinyxml2::XMLElement* oriX = parameters->GetDocument()->NewElement("x"); + tinyxml2::XMLElement* oriX = element->GetDocument()->NewElement("x"); oriX->SetText(latest_message_->pose.pose.orientation.x); // Set orientation x value orientationElement->InsertEndChild(oriX); - tinyxml2::XMLElement* oriY = parameters->GetDocument()->NewElement("y"); + tinyxml2::XMLElement* oriY = element->GetDocument()->NewElement("y"); oriY->SetText(latest_message_->pose.pose.orientation.y); // Set orientation y value orientationElement->InsertEndChild(oriY); - tinyxml2::XMLElement* oriZ = parameters->GetDocument()->NewElement("z"); + tinyxml2::XMLElement* oriZ = element->GetDocument()->NewElement("z"); oriZ->SetText(latest_message_->pose.pose.orientation.z); // Set orientation z value orientationElement->InsertEndChild(oriZ); - tinyxml2::XMLElement* oriW = parameters->GetDocument()->NewElement("w"); + tinyxml2::XMLElement* oriW = element->GetDocument()->NewElement("w"); oriW->SetText(latest_message_->pose.pose.orientation.w); // Set orientation w value orientationElement->InsertEndChild(oriW); - // Return the updated parameters element with Pose added - return parameters; + // Return the updated parameters element with Pose added as string + std::string result = convert_to_string(element); + + return result; }; }; } // namespace capabilities2_runner \ No newline at end of file diff --git a/capabilities2_runner_nav2/include/capabilities2_runner_nav2/waypoint_runner.hpp b/capabilities2_runner_nav2/include/capabilities2_runner_nav2/waypoint_runner.hpp index fbad70b..a28a2da 100644 --- a/capabilities2_runner_nav2/include/capabilities2_runner_nav2/waypoint_runner.hpp +++ b/capabilities2_runner_nav2/include/capabilities2_runner_nav2/waypoint_runner.hpp @@ -2,15 +2,19 @@ #include #include +#include #include #include #include +#include #include #include #include +using namespace std::chrono_literals; + namespace capabilities2_runner { @@ -47,17 +51,18 @@ class WayPointRunner : public ActionRunner */ virtual nav2_msgs::action::NavigateToPose::Goal generate_goal(tinyxml2::XMLElement* parameters) override { - parameters_ = parameters; - - parameters_->QueryDoubleAttribute("x", &x); - parameters_->QueryDoubleAttribute("y", &y); + parameters->QueryDoubleAttribute("x", &x); + parameters->QueryDoubleAttribute("y", &y); - RCLCPP_INFO(node_->get_logger(), "%s is triggered with x: %f and y: %f", run_config_.interface.c_str(), x, y); + RCLCPP_INFO(node_->get_logger(), "[%s] goal consist of x: %f and y: %f", run_config_.interface.c_str(), x, y); nav2_msgs::action::NavigateToPose::Goal goal_msg; geometry_msgs::msg::PoseStamped pose_msg; 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; @@ -69,15 +74,18 @@ class WayPointRunner : public ActionRunner } /** - * @brief This generate result function overrides the generate_result() function from ActionRunner(). Since - * this is not used in this context, this returns nullptr - * @param result message from FollowWaypoints action - * @return nullptr + * @brief This generate feedback function overrides the generate_feedback() function from ActionRunner() + * + * @param msg feedback message from the action server + * @return std::string of feedback information */ - virtual tinyxml2::XMLElement* - generate_result(const nav2_msgs::action::NavigateToPose::Result::SharedPtr& result) override + virtual std::string + generate_feedback(const typename nav2_msgs::action::NavigateToPose::Feedback::ConstSharedPtr msg) override { - return nullptr; + // std::string feedback = "x: " + std::to_string(msg->current_pose.pose.position.x) + + // " y: " + std::to_string(msg->current_pose.pose.position.y); + // return feedback; + return ""; } protected: @@ -85,6 +93,8 @@ class WayPointRunner : public ActionRunner std::string robot_base_frame_; /**The frame of the robot base*/ double x, y; /**Coordinate frame parameters*/ + + rclcpp::Client::SharedPtr get_state_client_; }; } // namespace capabilities2_runner diff --git a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_occupancy_runner.hpp b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_occupancy_runner.hpp index f3086f2..cea6c6d 100644 --- a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_occupancy_runner.hpp +++ b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_occupancy_runner.hpp @@ -46,8 +46,6 @@ class PromptOccupancyRunner : public ServiceRunner */ virtual typename prompt_msgs::srv::Prompt::Request generate_request(tinyxml2::XMLElement* parameters) override { - parameters_ = parameters; - tinyxml2::XMLElement* occupancyElement = parameters->FirstChildElement("OccupancyGrid"); tinyxml2::XMLPrinter printer; @@ -74,23 +72,6 @@ class PromptOccupancyRunner : public ServiceRunner return request; } - - /** - * @brief generate a typed erased response - * - * this method is used in a callback passed to the trigger caller to get type erased result - * from the service the reponse can be passed by the caller or ignored - * - * The pattern needs to be implemented in the derived class - * - * @param wrapped_result - * @return tinyxml2::XMLElement* - */ - virtual tinyxml2::XMLElement* - generate_response(const typename prompt_msgs::srv::Prompt::Response::SharedPtr& result) const override - { - return nullptr; - } }; } // namespace capabilities2_runner diff --git a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_request_runner.hpp b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_request_runner.hpp index c9c41c2..b75c5e5 100644 --- a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_request_runner.hpp +++ b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_request_runner.hpp @@ -47,8 +47,6 @@ class PromptPlanRequestRunner : public ServiceRunner */ virtual typename prompt_msgs::srv::Prompt::Request generate_request(tinyxml2::XMLElement* parameters) override { - parameters_ = parameters; - bool replan; parameters->QueryBoolAttribute("replan", &replan); @@ -56,13 +54,17 @@ class PromptPlanRequestRunner : public ServiceRunner if (!replan) { - request.prompt.prompt = "Build a xml plan based on the availbale capabilities to acheive mentioned task. Return only the xml plan without explanations or comments"; + request.prompt.prompt = "Build a xml plan based on the availbale capabilities to acheive mentioned task. Return " + "only the xml plan without explanations or comments"; } else { tinyxml2::XMLElement* failedElements = parameters->FirstChildElement("FailedElements"); - request.prompt.prompt = "Rebuild the xml plan based on the availbale capabilities to acheive mentioned task. Just give the xml plan without explanations or comments. These XML elements had incompatibilities. " + std::string(failedElements->GetText()) + "Recorrect them as well"; + request.prompt.prompt = "Rebuild the xml plan based on the availbale capabilities to acheive mentioned task. " + "Just give the xml plan without explanations or comments. These XML elements had " + "incompatibilities. " + + std::string(failedElements->GetText()) + "Recorrect them as well"; } prompt_msgs::msg::ModelOption modelOption1; @@ -81,25 +83,6 @@ class PromptPlanRequestRunner : public ServiceRunner return request; } - /** - * @brief generate a typed erased response - * - * this method is used in a callback passed to the trigger caller to get type erased result - * from the service the reponse can be passed by the caller or ignored - * - * The pattern needs to be implemented in the derived class - * - * @param wrapped_result - * @return tinyxml2::XMLElement* - */ - virtual tinyxml2::XMLElement* - generate_response(const prompt_msgs::srv::Prompt::Response::SharedPtr& result) const override - { - document_string = result->response.response; - - return nullptr; - } - /** * @brief Update on_success event parameters with new data if avaible. * @@ -111,21 +94,22 @@ class PromptPlanRequestRunner : public ServiceRunner * @param parameters pointer to the XMLElement containing parameters * @return pointer to the XMLElement containing updated parameters */ - virtual tinyxml2::XMLElement* update_on_success(tinyxml2::XMLElement* parameters) + virtual std::string update_on_success(std::string& parameters) { + tinyxml2::XMLElement* element = convert_to_xml(parameters); + + std::string document_string = response_->response.response; + // Create the plan element as a child of the existing parameters element - tinyxml2::XMLElement* textElement = parameters->GetDocument()->NewElement("ReceievdPlan"); - parameters->InsertEndChild(textElement); + tinyxml2::XMLElement* textElement = element->GetDocument()->NewElement("ReceievdPlan"); + element->InsertEndChild(textElement); textElement->SetText(document_string.c_str()); // Return the updated parameters element with Pose added - return parameters; - }; + std::string result = convert_to_string(element); - /** - * Document holder for received xml plan - */ - mutable std::string document_string; + return result; + }; }; } // namespace capabilities2_runner diff --git a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_response_runner.hpp b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_response_runner.hpp index 48e4c56..d6c3a36 100644 --- a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_response_runner.hpp +++ b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_response_runner.hpp @@ -45,8 +45,6 @@ class PromptPlanResponseRunner : public ActionRunnerFirstChildElement("ReceievdPlan"); auto goal_msg = capabilities2_msgs::action::Plan::Goal(); @@ -61,15 +59,16 @@ class PromptPlanResponseRunner : public ActionRunnerprogress; + return feedback; } /** @@ -83,13 +82,15 @@ class PromptPlanResponseRunner : public ActionRunnerSetAttribute("replan", true); + tinyxml2::XMLElement* element = convert_to_xml(parameters); + + element->SetAttribute("replan", true); // Create the failed elements element as a child of the existing parameters element - tinyxml2::XMLElement* failedElements = parameters->GetDocument()->NewElement("FailedElements"); - parameters->InsertEndChild(failedElements); + tinyxml2::XMLElement* failedElements = element->GetDocument()->NewElement("FailedElements"); + element->InsertEndChild(failedElements); std::string failedElementsString = ""; @@ -98,7 +99,9 @@ class PromptPlanResponseRunner : public ActionRunnerSetText(failedElementsString.c_str()); - return parameters; + std::string result = convert_to_string(element); + + return result; }; }; diff --git a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_pose_runner.hpp b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_pose_runner.hpp index 09ba97b..3dda94a 100644 --- a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_pose_runner.hpp +++ b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_pose_runner.hpp @@ -46,8 +46,6 @@ class PromptPoseRunner : public ServiceRunner */ virtual typename prompt_msgs::srv::Prompt::Request generate_request(tinyxml2::XMLElement* parameters) override { - parameters_ = parameters; - tinyxml2::XMLElement* poseElement = parameters->FirstChildElement("Pose"); tinyxml2::XMLPrinter printer; @@ -74,23 +72,6 @@ class PromptPoseRunner : public ServiceRunner return request; } - - /** - * @brief generate a typed erased response - * - * this method is used in a callback passed to the trigger caller to get type erased result - * from the service the reponse can be passed by the caller or ignored - * - * The pattern needs to be implemented in the derived class - * - * @param wrapped_result - * @return tinyxml2::XMLElement* - */ - virtual tinyxml2::XMLElement* - generate_response(const typename prompt_msgs::srv::Prompt::Response::SharedPtr& result) const override - { - return nullptr; - } }; } // namespace capabilities2_runner diff --git a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_text_runner.hpp b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_text_runner.hpp index 13713d1..a418f76 100644 --- a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_text_runner.hpp +++ b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_text_runner.hpp @@ -46,8 +46,6 @@ class PromptTextRunner : public ServiceRunner */ virtual typename prompt_msgs::srv::Prompt::Request generate_request(tinyxml2::XMLElement* parameters) override { - parameters_ = parameters; - tinyxml2::XMLElement* textElement = parameters->FirstChildElement("Text"); tinyxml2::XMLPrinter printer; @@ -74,23 +72,6 @@ class PromptTextRunner : public ServiceRunner return request; } - - /** - * @brief generate a typed erased response - * - * this method is used in a callback passed to the trigger caller to get type erased result - * from the service the reponse can be passed by the caller or ignored - * - * The pattern needs to be implemented in the derived class - * - * @param wrapped_result - * @return tinyxml2::XMLElement* - */ - virtual tinyxml2::XMLElement* - generate_response(const typename prompt_msgs::srv::Prompt::Response::SharedPtr& result) const override - { - return nullptr; - } }; } // namespace capabilities2_runner diff --git a/capabilities2_server/CMakeLists.txt b/capabilities2_server/CMakeLists.txt index cb79c7f..3eb130c 100644 --- a/capabilities2_server/CMakeLists.txt +++ b/capabilities2_server/CMakeLists.txt @@ -19,6 +19,7 @@ find_package(pluginlib REQUIRED) find_package(rclcpp_components REQUIRED) find_package(capabilities2_msgs REQUIRED) find_package(capabilities2_runner REQUIRED) +find_package(backward_ros REQUIRED) find_package(PkgConfig) pkg_check_modules(UUID REQUIRED uuid) @@ -43,8 +44,10 @@ include_directories(include ${UUID_INCLUDE_DIRS} ) +# Component based implementation that compiles as a library + add_library(${PROJECT_NAME} SHARED - src/capabilities_server.cpp + src/capabilities_server_comp.cpp ) target_link_libraries(${PROJECT_NAME} @@ -69,19 +72,50 @@ ament_target_dependencies(${PROJECT_NAME} rclcpp_components_register_node(${PROJECT_NAME} PLUGIN "capabilities2_server::CapabilitiesServer" - EXECUTABLE capabilities2_server_node + EXECUTABLE capabilities2_server_component_node ) -ament_export_targets(capabilities2_server_node) +ament_export_targets(capabilities2_server_component_node) install(TARGETS ${PROJECT_NAME} - EXPORT capabilities2_server_node + EXPORT capabilities2_server_component_node ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin ) +# Non Component based implementation that compiles as a executable + +add_executable(${PROJECT_NAME}_node + src/capabilities_server_node.cpp +) + +target_link_libraries(${PROJECT_NAME}_node + ${UUID_LIBRARIES} + ${TINYXML2_LIB} + ${SQLITE3_LIBRARIES} + ${YAML_CPP_LIBRARIES} +) + +ament_target_dependencies(${PROJECT_NAME}_node + rclcpp + rclcpp_action + bondcpp + pluginlib + rclcpp_components + capabilities2_msgs + capabilities2_runner + SQLite3 + yaml-cpp + UUID +) + +install(TARGETS ${PROJECT_NAME}_node + DESTINATION lib/${PROJECT_NAME} +) + # make test executable + add_executable(test_capabilities_server test/test.cpp) target_link_libraries(test_capabilities_server ${PROJECT_NAME} diff --git a/capabilities2_server/include/capabilities2_server/bond_cache.hpp b/capabilities2_server/include/capabilities2_server/bond_cache.hpp index 296ddcc..4895107 100644 --- a/capabilities2_server/include/capabilities2_server/bond_cache.hpp +++ b/capabilities2_server/include/capabilities2_server/bond_cache.hpp @@ -19,7 +19,7 @@ namespace capabilities2_server class BondCache { public: - BondCache(const std::string& bonds_topic = "~/bonds") : bonds_topic_(bonds_topic) + BondCache(const std::string& bonds_topic = "/capabilities/bond") : bonds_topic_(bonds_topic) { } @@ -124,6 +124,8 @@ class BondCache live_bonds_[bond_id] = std::make_unique(bonds_topic_, bond_id, node, on_broken, on_formed); // start the bond + live_bonds_[bond_id]->setHeartbeatPeriod(0.10); + live_bonds_[bond_id]->setHeartbeatTimeout(10.0); live_bonds_[bond_id]->start(); } diff --git a/capabilities2_server/include/capabilities2_server/capabilities_api.hpp b/capabilities2_server/include/capabilities2_server/capabilities_api.hpp index 45f2bc0..55acc3b 100644 --- a/capabilities2_server/include/capabilities2_server/capabilities_api.hpp +++ b/capabilities2_server/include/capabilities2_server/capabilities_api.hpp @@ -280,7 +280,7 @@ class CapabilitiesAPI on_failure_parameters, on_success_capability, on_success_parameters, on_stopped_capability, on_stopped_parameters); - RCLCPP_WARN(node_logging_interface_ptr_->get_logger(), "successfully set triggers"); + RCLCPP_INFO(node_logging_interface_ptr_->get_logger(), "successfully set triggers"); } catch (const capabilities2_runner::runner_exception& e) { diff --git a/capabilities2_server/include/capabilities2_server/capabilities_server.hpp b/capabilities2_server/include/capabilities2_server/capabilities_server.hpp index b5c5edd..517c065 100644 --- a/capabilities2_server/include/capabilities2_server/capabilities_server.hpp +++ b/capabilities2_server/include/capabilities2_server/capabilities_server.hpp @@ -60,7 +60,7 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI { public: CapabilitiesServer(const rclcpp::NodeOptions& options = rclcpp::NodeOptions()) - : Node("capabilities", options), CapabilitiesAPI() + : Node("capabilities2", options), CapabilitiesAPI() { // params interface // loop rate diff --git a/capabilities2_server/include/capabilities2_server/runner_cache.hpp b/capabilities2_server/include/capabilities2_server/runner_cache.hpp index 1ebd874..b702409 100644 --- a/capabilities2_server/include/capabilities2_server/runner_cache.hpp +++ b/capabilities2_server/include/capabilities2_server/runner_cache.hpp @@ -98,17 +98,15 @@ class RunnerCache */ void trigger_runner(const std::string& capability, const std::string& parameters) { - tinyxml2::XMLDocument doc; - doc.Parse(parameters.c_str()); - tinyxml2::XMLElement* xml_parameters = doc.FirstChildElement(); - // is the runner in the cache if (running(capability)) { - runner_cache_[capability]->trigger(xml_parameters); + runner_cache_[capability]->trigger(parameters); } else { + RCLCPP_ERROR(node_logging_interface_ptr_->get_logger(), "Runner not found for capability: %s", + capability.c_str()); throw capabilities2_runner::runner_exception("capability runner not found: " + capability); } } @@ -135,98 +133,25 @@ class RunnerCache { capabilities2_runner::event_opts event_options; - if (on_started_capability != "") - { - event_options.on_started = [this, &on_started_capability, &capability](tinyxml2::XMLElement* parameters) { - RCLCPP_INFO(node_logging_interface_ptr_->get_logger(), "%s is triggering %s on_start", capability.c_str(), - on_started_capability.c_str()); - runner_cache_[on_started_capability]->trigger(parameters); - }; - - tinyxml2::XMLDocument doc; - doc.Parse(on_started_parameters.c_str()); - event_options.on_started_param = doc.FirstChildElement(); - } - else - { - event_options.on_started = [this, &capability](tinyxml2::XMLElement* parameters) { - RCLCPP_INFO(node_logging_interface_ptr_->get_logger(), "%s does not implement on_start event", - capability.c_str()); - }; - - event_options.on_started_param = nullptr; - } - - if (on_failure_capability != "") - { - event_options.on_failure = [this, &on_failure_capability, &capability](tinyxml2::XMLElement* parameters) { - RCLCPP_INFO(node_logging_interface_ptr_->get_logger(), "%s is triggering %s on_failure", capability.c_str(), - on_failure_capability.c_str()); - runner_cache_[on_failure_capability]->trigger(parameters); - }; - - tinyxml2::XMLDocument doc; - doc.Parse(on_failure_parameters.c_str()); - event_options.on_failure_param = doc.FirstChildElement(); - } - else - { - event_options.on_failure = [this, &capability](tinyxml2::XMLElement* parameters) { - RCLCPP_INFO(node_logging_interface_ptr_->get_logger(), "%s does not implement on_failure event", - capability.c_str()); - } ; - - event_options.on_failure_param = nullptr; - } - - if (on_success_capability != "") - { - event_options.on_success = [this, &on_success_capability, &capability](tinyxml2::XMLElement* parameters) { - runner_cache_[on_success_capability]->trigger(parameters); - RCLCPP_INFO(node_logging_interface_ptr_->get_logger(), "%s is triggering %s on_success", capability.c_str(), - on_success_capability.c_str()); - }; - - tinyxml2::XMLDocument doc; - doc.Parse(on_success_parameters.c_str()); - event_options.on_success_param = doc.FirstChildElement(); - } - else - { - event_options.on_success = [this, &capability](tinyxml2::XMLElement* parameters) { - RCLCPP_INFO(node_logging_interface_ptr_->get_logger(), "%s does not implement on_success event", - capability.c_str()); - } ; - - event_options.on_success_param = nullptr; - } - - if (on_stopped_capability != "") - { - event_options.on_stopped = [this, &on_stopped_capability, &capability](tinyxml2::XMLElement* parameters) { - runner_cache_[on_stopped_capability]->trigger(parameters); - RCLCPP_INFO(node_logging_interface_ptr_->get_logger(), "%s is triggering %s on_stopped", capability.c_str(), - on_stopped_capability.c_str()); - }; - - tinyxml2::XMLDocument doc; - doc.Parse(on_stopped_parameters.c_str()); - event_options.on_stopped_param = doc.FirstChildElement(); - } - else - { - event_options.on_stopped = [this, &capability](tinyxml2::XMLElement* parameters) { - RCLCPP_INFO(node_logging_interface_ptr_->get_logger(), "%s does not implement on_stopped event", - capability.c_str()); - } ; - - event_options.on_stopped_param = nullptr; - } - - int event_count = runner_cache_[capability]->attach_events(event_options); - - std::string event_name = capability + "_" + std::to_string(event_count); - event_cache_[event_name] = event_options; + event_options.on_started = on_started_capability; + event_options.on_failure = on_failure_capability; + event_options.on_success = on_success_capability; + event_options.on_stopped = on_stopped_capability; + + event_options.on_started_param = on_started_parameters; + event_options.on_failure_param = on_failure_parameters; + event_options.on_success_param = on_success_parameters; + event_options.on_stopped_param = on_stopped_parameters; + + int event_count = runner_cache_[capability]->attach_events( + event_options, std::bind(&capabilities2_server::RunnerCache::trigger_runner, this, std::placeholders::_1, + std::placeholders::_2)); + + RCLCPP_INFO(node_logging_interface_ptr_->get_logger(), + "Configured triggers for capability %s: \n\tStarted: %s \n\tFailure: %s \n\tSuccess: %s \n\tStopped: " + "%s\n", + capability.c_str(), on_started_capability.c_str(), on_failure_capability.c_str(), + on_success_capability.c_str(), on_stopped_capability.c_str()); } /** @@ -336,9 +261,6 @@ class RunnerCache // capability / provider specs -> runner std::map> runner_cache_; - // map events to capabilities - std::map event_cache_; - // runner plugin loader pluginlib::ClassLoader runner_loader_; diff --git a/capabilities2_server/launch/server.launch.py b/capabilities2_server/launch/server.launch.py index 4f70a3e..6879e11 100644 --- a/capabilities2_server/launch/server.launch.py +++ b/capabilities2_server/launch/server.launch.py @@ -5,8 +5,6 @@ import os from launch import LaunchDescription from launch_ros.actions import Node -from launch_ros.actions import ComposableNodeContainer -from launch_ros.descriptions import ComposableNode from ament_index_python.packages import get_package_share_directory @@ -20,32 +18,16 @@ def generate_launch_description(): server_config = os.path.join(get_package_share_directory('capabilities2_server'), 'config', 'capabilities.yaml') # create bridge composition - capabilities = ComposableNodeContainer( - name='capabilities2_container', - namespace='', - package='rclcpp_components', - executable='component_container', - # prefix=['xterm -e gdb -ex run --args'], # Add GDB debugging prefix - arguments=['--ros-args', '--log-level', 'info'], - composable_node_descriptions=[ - ComposableNode( - package='capabilities2_server', - plugin='capabilities2_server::CapabilitiesServer', - name='capabilities', - parameters=[server_config] - ) - ] - ) - - # create launch proxy node - launch_proxy = Node( - package='capabilities2_launch_py', - executable='capabilities2_launch_py', - name='capabilities2_launch_py' + capabilities2 = Node( + package='capabilities2_server', + executable='capabilities2_server_node', + name='capabilities', + parameters=[server_config], + output='screen', + arguments=['--ros-args', '--log-level', 'info'] ) # return return LaunchDescription([ - capabilities, - launch_proxy + capabilities2 ]) diff --git a/capabilities2_server/launch/server_composed.launch.py b/capabilities2_server/launch/server_composed.launch.py new file mode 100644 index 0000000..498f824 --- /dev/null +++ b/capabilities2_server/launch/server_composed.launch.py @@ -0,0 +1,43 @@ +''' +capabilities2_server launch file +''' + +import os +from launch import LaunchDescription +from launch_ros.actions import Node +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + """Generate launch description for capabilities2 server + + Returns: + LaunchDescription: The launch description for capabilities2 server + """ + # load config file + server_config = os.path.join(get_package_share_directory('capabilities2_server'), 'config', 'capabilities.yaml') + + # create bridge composition + capabilities = ComposableNodeContainer( + name='capabilities2_container', + namespace='', + package='rclcpp_components', + executable='component_container_mt', + # prefix=['xterm -e gdb -ex run --args'], # Add GDB debugging prefix + arguments=['--ros-args', '--log-level', 'info'], + composable_node_descriptions=[ + ComposableNode( + package='capabilities2_server', + plugin='capabilities2_server::CapabilitiesServer', + name='capabilities', + parameters=[server_config] + ) + ] + ) + + # return + return LaunchDescription([ + capabilities + ]) diff --git a/capabilities2_server/package.xml b/capabilities2_server/package.xml index ffa1e48..e9fb76d 100644 --- a/capabilities2_server/package.xml +++ b/capabilities2_server/package.xml @@ -28,6 +28,7 @@ tinyxml2 libsqlite3-dev uuid + backward_ros rclpy launch_ros diff --git a/capabilities2_server/src/capabilities_server.cpp b/capabilities2_server/src/capabilities_server_comp.cpp similarity index 94% rename from capabilities2_server/src/capabilities_server.cpp rename to capabilities2_server/src/capabilities_server_comp.cpp index 8c8eb6f..efbad0e 100644 --- a/capabilities2_server/src/capabilities_server.cpp +++ b/capabilities2_server/src/capabilities_server_comp.cpp @@ -1,4 +1,4 @@ #include #include -RCLCPP_COMPONENTS_REGISTER_NODE(capabilities2_server::CapabilitiesServer) +RCLCPP_COMPONENTS_REGISTER_NODE(capabilities2_server::CapabilitiesServer) \ No newline at end of file diff --git a/capabilities2_server/src/capabilities_server_node.cpp b/capabilities2_server/src/capabilities_server_node.cpp new file mode 100644 index 0000000..48e5bb6 --- /dev/null +++ b/capabilities2_server/src/capabilities_server_node.cpp @@ -0,0 +1,22 @@ +#include + +int main(int argc, char** argv) +{ + // Initialize ROS 2 + rclcpp::init(argc, argv); + + // Create the node object and executor object + auto node = std::make_shared(); + auto exec = std::make_shared(); + + // Add the node to the executor + exec->add_node(node); + + // Spin the executor + exec->spin(); + + // Shutdown ROS 2 + rclcpp::shutdown(); + + return 0; +} \ No newline at end of file diff --git a/docs/nav2_setup.md b/docs/nav2_setup.md index 1b27fe5..2a44385 100644 --- a/docs/nav2_setup.md +++ b/docs/nav2_setup.md @@ -2,8 +2,16 @@ ## Install nav2 stack +Run the following commands to install nav2 stack + +```bash +sudo apt install ros-humble-navigation2 ros-humble-nav2-bringup +``` + +## Install slam-toolbox + ```bash -sudo apt install ros-$ROS_DISTRO-navigation2 ros-$ROS_DISTRO-nav2-bringup ros-$ROS_DISTRO-slam-toolbox +sudo apt install ros-$ROS_DISTRO-slam-toolbox ``` ## Clone configuration @@ -16,8 +24,17 @@ git clone https://github.com/CollaborativeRoboticsLab/nav_stack.git ``` ## Turtlebot3 Simulation (Optional) + If using a simulated turtlebot3 for testing, install using following commands. ```bash sudo apt install ros-$ROS_DISTRO-turtlebot3* +``` + +## Dependency Installation + +Move to workspace root and run the following command to install dependencies + +```bash +rosdep install --from-paths src --ignore-src -r -y ``` \ No newline at end of file