completed prompt-testing

Author: KalanaRatnayake

capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric.hpp

@@ -96,6 +96,8 @@ class CapabilitiesFabric : public rclcpp::Node
 
     feedback_msg = std::make_shared<Plan::Feedback>();
     result_msg = std::make_shared<Plan::Result>();
+
+    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<Plan::Feedback>();
     auto result = std::make_shared<Plan::Result>();
 
-    // intialize a vector to accomodate elements from both
-    std::vector<std::string> 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<BondClient>(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<BondClient>(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<int, capabilities2::node_t>& capabilities)
   {
+    std::string capability = capabilities[freed_capabilities_].source.runner;
+
     auto request_free = std::make_shared<FreeCapability::Request>();
     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<BondClient> bond_client_;
+  std::map<std::string, std::shared_ptr<BondClient>> bond_client_cache_;
 
   /** Handles status message sending and printing to logging */
   std::shared_ptr<StatusClient> status_;

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<CompleteFabric::Request> request, std::shared_ptr<CompleteFabric::Response> 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");
     }

capabilities2_fabric/include/capabilities2_fabric/utils/bond_client.hpp

@@ -1,4 +1,5 @@
 #pragma once
+#include <string>
 #include <bondcpp/bond.hpp>
 #include <rclcpp/rclcpp.hpp>
 
@@ -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<bond::Bond>(bonds_topic_, bond_id_, node_, std::bind(&BondClient::on_broken, this), std::bind(&BondClient::on_formed, this));
+    bond_ = std::make_unique<bond::Bond>(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::Bond> bond_;

capabilities2_fabric/plans/prompt_4.xml

@@ -3,7 +3,7 @@
     <Control name="sequential">
         <Event name="std_capabilities/CapabilityGetRunner" provider="std_capabilities/CapabilityGetRunner"/>
         <Event name="std_capabilities/PromptCapabilityRunner" provider="std_capabilities/PromptCapabilityRunner" />
-        <Event name="std_capabilities/PromptPlanRunner" provider="std_capabilities/PromptPlanRunner" replan="false" task="move to x:1 and y:2 position" />
+        <Event name="std_capabilities/PromptPlanRunner" provider="std_capabilities/PromptPlanRunner" replan="false" task="move to x:0.5 and y:1 position" />
         <Event name="std_capabilities/FabricSetPlanRunner" provider="std_capabilities/FabricSetPlanRunner"/>
     </Control>
 </Plan>

capabilities2_runner/include/capabilities2_runner/action_runner.hpp

@@ -115,6 +115,8 @@ class ActionRunner : public RunnerBase
         throw runner_exception(e.what());
       }
     }
+
+    info_("stopping runner");
   }
 
   /**

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

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:

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:

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;
     }

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);