diff --git a/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric.hpp b/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric.hpp index 04c9120..b43f799 100644 --- a/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric.hpp +++ b/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric.hpp @@ -96,6 +96,8 @@ class CapabilitiesFabric : public rclcpp::Node feedback_msg = std::make_shared(); result_msg = std::make_shared(); + + need_reset_ = false; } private: @@ -141,7 +143,10 @@ class CapabilitiesFabric : public rclcpp::Node status_->error("Received the request to cancel the plan"); (void)goal_handle; - bond_client_->stop(); + for (auto& [bond_id, bond_client] : bond_client_cache_) + { + bond_client->stop(); + } return rclcpp_action::CancelResponse::ACCEPT; } @@ -173,6 +178,17 @@ class CapabilitiesFabric : public rclcpp::Node status_->info("Plan after adding closing event :\n\n " + modified_plan); + // if (need_reset_) + // { + // free_capability_all(connection_map); + // need_reset_ = false; + // } + + interface_list.clear(); + providers_list.clear(); + rejected_list.clear(); + connection_map.clear(); + expected_providers_ = 0; completed_providers_ = 0; @@ -181,6 +197,7 @@ class CapabilitiesFabric : public rclcpp::Node expected_capabilities_ = 0; completed_capabilities_ = 0; + freed_capabilities_ = 0; expected_configurations_ = 0; completed_configurations_ = 0; @@ -406,7 +423,7 @@ class CapabilitiesFabric : public rclcpp::Node process_feedback("Connection extraction successful"); // estasblish the bond with the server - establish_bond(); + request_bond(); } /** @@ -419,10 +436,6 @@ class CapabilitiesFabric : public rclcpp::Node auto feedback = std::make_shared(); auto result = std::make_shared(); - // intialize a vector to accomodate elements from both - std::vector tag_list(interface_list.size() + control_tag_list.size()); - std::merge(interface_list.begin(), interface_list.end(), control_tag_list.begin(), control_tag_list.end(), tag_list.begin()); - // verify whether document got 'plan' tags if (!xml_parser::check_plan_tag(document)) { @@ -449,10 +462,10 @@ class CapabilitiesFabric : public rclcpp::Node } /** - * @brief establish the bond with capabilities2 server + * @brief Request the bond from the capabilities2 server * */ - void establish_bond() + void request_bond() { process_feedback("Requesting bond id"); @@ -469,18 +482,40 @@ class CapabilitiesFabric : public rclcpp::Node auto response = future.get(); bond_id_ = response->bond_id; - process_feedback("Received the bond id : " + bond_id_); - bond_client_ = std::make_unique(shared_from_this(), bond_id_); - bond_client_->start(); + establish_bond(); + }); + } - expected_capabilities_ = connection_map.size(); + /** + * @brief establish the bond with capabilities2 server + * + */ + void establish_bond() + { + bond_client_cache_[bond_id_] = std::make_unique(shared_from_this(), bond_id_); + bond_client_cache_[bond_id_]->start(); - process_feedback("Requsting start of " + std::to_string(expected_capabilities_) + " capabilities"); + process_feedback("Bond sucessfully established with bond id : " + bond_id_); - use_capability(connection_map); - }); + if (bond_client_cache_.size() > 1) + { + for (auto& [old_bond_id, bond_client] : bond_client_cache_) + { + if (old_bond_id != bond_id_) + { + bond_client->stop(); + process_feedback("Stopping and removing old bond with id : " + old_bond_id); + } + } + } + + expected_capabilities_ = connection_map.size(); + + process_feedback("Requsting start of " + std::to_string(expected_capabilities_) + " capabilities"); + + use_capability(connection_map); } /** @@ -511,17 +546,17 @@ class CapabilitiesFabric : public rclcpp::Node process_result("Failed to Use capability " + capability + " from " + provider + ". Server Execution Cancelled"); // release all capabilities that were used since not all started successfully - for (const auto& [key, value] : connection_map) + free_capability_all(connection_map); + + for (auto& [bond_id, bond_client] : bond_client_cache_) { - process_feedback("Freeing capability of Node " + std::to_string(key) + " named " + value.source.runner); - free_capability(value.source.runner); + bond_client->stop(); } - - bond_client_->stop(); return; } completed_capabilities_++; + need_reset_ = true; auto response = future.get(); @@ -546,12 +581,14 @@ class CapabilitiesFabric : public rclcpp::Node } /** - * @brief Request use of capability from capabilities2 server + * @brief Free all started capabilities in the capabilities map * - * @param capability capability name to be started + * @param capabilities map of capabilities to be freed */ - void free_capability(const std::string& capability) + void free_capability_all(std::map& capabilities) { + std::string capability = capabilities[freed_capabilities_].source.runner; + auto request_free = std::make_shared(); request_free->capability = capability; request_free->bond_id = bond_id_; @@ -566,6 +603,18 @@ class CapabilitiesFabric : public rclcpp::Node auto response = future.get(); process_feedback("Successfully freed capability " + capability, true); + + freed_capabilities_++; + + // Check if all expected calls are completed before calling verify_plan + if (freed_capabilities_ == completed_capabilities_) + { + process_feedback("All started capabilities have been freed."); + } + else + { + free_capability_all(connection_map); + } }); } @@ -754,6 +803,7 @@ class CapabilitiesFabric : public rclcpp::Node /** flag to select loading from file or accepting via action server */ bool read_file; + bool need_reset_; int expected_interfaces_; int completed_interfaces_; @@ -763,6 +813,7 @@ class CapabilitiesFabric : public rclcpp::Node int expected_capabilities_; int completed_capabilities_; + int freed_capabilities_; int expected_configurations_; int completed_configurations_; @@ -771,7 +822,7 @@ class CapabilitiesFabric : public rclcpp::Node std::string bond_id_; /** Manages bond between capabilities server and this client */ - std::shared_ptr bond_client_; + std::map> bond_client_cache_; /** Handles status message sending and printing to logging */ std::shared_ptr status_; diff --git a/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric_client.hpp b/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric_client.hpp index d5462f8..5ef662d 100644 --- a/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric_client.hpp +++ b/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric_client.hpp @@ -176,11 +176,11 @@ class CapabilitiesFabricClient : public rclcpp::Node if (result.result->success) { - status_->info("Plan executed successfully"); + status_->info("Plan launched successfully"); } else { - status_->error("Plan failed to complete"); + status_->error("Plan failed to launch"); if (result.result->failed_elements.size() > 0) { @@ -199,6 +199,7 @@ class CapabilitiesFabricClient : public rclcpp::Node { if (fabric_state == Status::RUNNING) { + status_->info("Plan canncelling requested"); this->planner_client_->async_cancel_goal(goal_handle_); } @@ -208,7 +209,7 @@ class CapabilitiesFabricClient : public rclcpp::Node void setCompleteCallback(const std::shared_ptr request, std::shared_ptr response) { fabric_state = Status::COMPLETED; - + status_->info("Plan completed successfully"); completed_ = true; cv_.notify_all(); } @@ -267,7 +268,9 @@ class CapabilitiesFabricClient : public rclcpp::Node plan_queue.push_back(request->plan); - if (fabric_state == Status::RUNNING) + status_->info("Plan queued and waiting for execution"); + + if ((fabric_state == Status::RUNNING) or (fabric_state == Status::LAUNCHED)) { status_->info("Prior plan under exeution. Will defer the new plan"); } diff --git a/capabilities2_fabric/include/capabilities2_fabric/utils/bond_client.hpp b/capabilities2_fabric/include/capabilities2_fabric/utils/bond_client.hpp index 401391e..de20de9 100644 --- a/capabilities2_fabric/include/capabilities2_fabric/utils/bond_client.hpp +++ b/capabilities2_fabric/include/capabilities2_fabric/utils/bond_client.hpp @@ -1,4 +1,5 @@ #pragma once +#include #include #include @@ -7,7 +8,7 @@ class BondClient public: BondClient(rclcpp::Node::SharedPtr node, const std::string& bond_id, const std::string& bonds_topic = "/capabilities/bond") { - bonds_topic_ = bonds_topic; + topic_ = bonds_topic; bond_id_ = bond_id; node_ = node; } @@ -16,8 +17,7 @@ class BondClient { RCLCPP_INFO(node_->get_logger(), "[BondClient] creating bond to capabilities server"); - bond_ = - std::make_unique(bonds_topic_, bond_id_, node_, std::bind(&BondClient::on_broken, this), std::bind(&BondClient::on_formed, this)); + bond_ = std::make_unique(topic_, bond_id_, node_, std::bind(&BondClient::on_broken, this), std::bind(&BondClient::on_formed, this)); bond_->setHeartbeatPeriod(0.10); bond_->setHeartbeatTimeout(10.0); @@ -59,7 +59,7 @@ class BondClient std::string bond_id_; /** Bond topic to be published */ - std::string bonds_topic_; + std::string topic_; /** Heart beat bond with capabilities server */ std::shared_ptr bond_; diff --git a/capabilities2_fabric/plans/prompt_4.xml b/capabilities2_fabric/plans/prompt_4.xml index b95119b..dde90ac 100644 --- a/capabilities2_fabric/plans/prompt_4.xml +++ b/capabilities2_fabric/plans/prompt_4.xml @@ -3,7 +3,7 @@ - + \ No newline at end of file diff --git a/capabilities2_runner/include/capabilities2_runner/action_runner.hpp b/capabilities2_runner/include/capabilities2_runner/action_runner.hpp index 43f0fde..1c4b38f 100644 --- a/capabilities2_runner/include/capabilities2_runner/action_runner.hpp +++ b/capabilities2_runner/include/capabilities2_runner/action_runner.hpp @@ -115,6 +115,8 @@ class ActionRunner : public RunnerBase throw runner_exception(e.what()); } } + + info_("stopping runner"); } /** diff --git a/capabilities2_runner/include/capabilities2_runner/launch_runner.hpp b/capabilities2_runner/include/capabilities2_runner/launch_runner.hpp index f2f6ea5..7673f19 100644 --- a/capabilities2_runner/include/capabilities2_runner/launch_runner.hpp +++ b/capabilities2_runner/include/capabilities2_runner/launch_runner.hpp @@ -124,6 +124,8 @@ class LaunchRunner : public RunnerBase RCLCPP_INFO(node_->get_logger(), "Request to launch %s from %s succeeded ", launch_name.c_str(), package_name.c_str()); }); + + info_("stopping runner"); } // throw on trigger function diff --git a/capabilities2_runner/include/capabilities2_runner/service_runner.hpp b/capabilities2_runner/include/capabilities2_runner/service_runner.hpp index d744cf6..0fd505b 100644 --- a/capabilities2_runner/include/capabilities2_runner/service_runner.hpp +++ b/capabilities2_runner/include/capabilities2_runner/service_runner.hpp @@ -143,6 +143,8 @@ class ServiceRunner : public RunnerBase info_("on_stopped", -1, events[execute_id].on_stopped, EventType::STOPPED); triggerFunction_(events[execute_id].on_stopped, update_on_stopped(events[execute_id].on_stopped_param)); } + + info_("stopping runner"); } protected: diff --git a/capabilities2_runner/include/capabilities2_runner/topic_runner.hpp b/capabilities2_runner/include/capabilities2_runner/topic_runner.hpp index 1d28990..bc5001b 100644 --- a/capabilities2_runner/include/capabilities2_runner/topic_runner.hpp +++ b/capabilities2_runner/include/capabilities2_runner/topic_runner.hpp @@ -122,6 +122,8 @@ class TopicRunner : public RunnerBase info_("on_stopped", -1, events[execute_id].on_stopped, EventType::STOPPED); triggerFunction_(events[execute_id].on_stopped, update_on_stopped(events[execute_id].on_stopped_param)); } + + info_("stopping runner"); } protected: diff --git a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_runner.hpp b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_runner.hpp index 65b0138..2cab1eb 100644 --- a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_runner.hpp +++ b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_runner.hpp @@ -43,7 +43,7 @@ class PromptPlanRunner : public PromptServiceRunner if (!replan) { prompt = "Build a xml plan based on the availbale capabilities to acheive mentioned task of " + taskString + - ". Return only the xml plan without explanations or comments"; + ". Return only the xml plan without explanations or comments."; flush = true; } diff --git a/capabilities2_server/include/capabilities2_server/bond_cache.hpp b/capabilities2_server/include/capabilities2_server/bond_cache.hpp index 4895107..4705371 100644 --- a/capabilities2_server/include/capabilities2_server/bond_cache.hpp +++ b/capabilities2_server/include/capabilities2_server/bond_cache.hpp @@ -71,6 +71,7 @@ class BondCache { // remove bond id from capability entry auto it = std::find(bond_cache_[capability].begin(), bond_cache_[capability].end(), bond_id); + if (it != bond_cache_[capability].end()) { bond_cache_[capability].erase(it); diff --git a/capabilities2_server/include/capabilities2_server/capabilities_api.hpp b/capabilities2_server/include/capabilities2_server/capabilities_api.hpp index e95cafb..cc401e2 100644 --- a/capabilities2_server/include/capabilities2_server/capabilities_api.hpp +++ b/capabilities2_server/include/capabilities2_server/capabilities_api.hpp @@ -164,7 +164,7 @@ class CapabilitiesAPI // FIXME: this unrolls the dependency tree from the bottom up but should probably be top down for (const auto& run : running.dependencies) { - print_("freeing dependency: " + run.interface, true, false); + print_("freeing dependency: " + run.interface + "of : " + capability, true, false); // remove the internal 'use' bond for the capability dependency unbind_dependency(run.interface); @@ -180,7 +180,7 @@ class CapabilitiesAPI // this will implicitly stop the runner try { - runner_cache_.remove_runner(capability); + runner_cache_.remove_runner(capability); } catch (const capabilities2_runner::runner_exception& e) { @@ -368,7 +368,7 @@ class CapabilitiesAPI } // couldn't parse unknown capability type - print_("unknown capability type: " +spec.type, true, true); + print_("unknown capability type: " + spec.type, true, true); } // query api diff --git a/capabilities2_server/include/capabilities2_server/capabilities_server.hpp b/capabilities2_server/include/capabilities2_server/capabilities_server.hpp index 1d4187e..c666d15 100644 --- a/capabilities2_server/include/capabilities2_server/capabilities_server.hpp +++ b/capabilities2_server/include/capabilities2_server/capabilities_server.hpp @@ -716,9 +716,6 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI // loop hz double loop_hz_; - /** capabilities_fabric launch thread */ - std::shared_ptr fabric_launch_thread; - // publishers // event publisher rclcpp::Publisher::SharedPtr event_pub_; diff --git a/capabilities2_server/include/capabilities2_server/runner_cache.hpp b/capabilities2_server/include/capabilities2_server/runner_cache.hpp index 784334f..416612c 100644 --- a/capabilities2_server/include/capabilities2_server/runner_cache.hpp +++ b/capabilities2_server/include/capabilities2_server/runner_cache.hpp @@ -184,10 +184,10 @@ class RunnerCache } // reset the runner pointer - runner_cache_[capability].reset(); + // runner_cache_[capability].reset(); // remove the runner from map - runner_cache_.erase(capability); + // runner_cache_.erase(capability); } /**