Adding threads to fabric

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
 )
 

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
 

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
 

capabilities2_fabric/include/capabilities2_executor/capabilities_file_parser.hpp → capabilities2_fabric/include/capabilities2_fabric/capabilities_file_parser.hpp

@@ -10,7 +10,7 @@
 #include <rclcpp/rclcpp.hpp>
 #include <rclcpp_action/rclcpp_action.hpp>
 
-#include <capabilities2_executor/capabilities_xml_parser.hpp>
+#include <capabilities2_fabric/capabilities_xml_parser.hpp>
 #include <capabilities2_msgs/action/plan.hpp>
 
 /**
@@ -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<capabilities2_msgs::action::Plan>(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<capabilities2_msgs::action::Plan>::SharedPtr client_ptr_;
-
-  /** action server */
-  rclcpp::TimerBase::SharedPtr timer_;
 };

capabilities2_fabric/include/capabilities2_executor/capabilities_xml_parser.hpp → capabilities2_fabric/include/capabilities2_fabric/capabilities_xml_parser.hpp

@@ -2,7 +2,7 @@
 #include <string>
 #include <vector>
 #include <rclcpp/rclcpp.hpp>
-#include <capabilities2_executor/structs/connection.hpp>
+#include <capabilities2_fabric/utils/connection.hpp>
 
 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;
   }
 }

capabilities2_fabric/include/capabilities2_fabric/utils/bond_client.hpp

@@ -0,0 +1,65 @@
+#include <bondcpp/bond.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+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<bond::Bond>(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::Bond> bond_;
+};

capabilities2_fabric/package.xml

@@ -13,6 +13,7 @@
   <depend>rclcpp</depend>
   <depend>capabilities2_msgs</depend>
   <depend>rclcpp_action</depend>
+  <depend>backward_ros</depend> 
 
   <!-- system dependency -->
   <depend>tinyxml2</depend>

capabilities2_fabric/plans/navigation_1.xml

@@ -1,6 +1,6 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <Plan>
     <Control name="sequential">
-        <Event name="std_capabilities/WaypointRunner" provider="std_capabilities/WaypointRunner" x="0.5" y="0.5" />
+        <Event name="std_capabilities/WaypointRunner" provider="std_capabilities/WaypointRunner" x="0.0" y="0.1" />
     </Control>
 </Plan>

capabilities2_fabric/plans/navigation_2.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <Plan>
     <Control name="sequential">
-        <Event name="std_capabilities/WaypointRunner" provider="std_capabilities/WaypointRunner" x="0.5" y="0.5" />
-        <Event name="std_capabilities/WaypointRunner" provider="std_capabilities/WaypointRunner" x="1.0" y="0.5" />
+        <Event name="std_capabilities/WaypointRunner" provider="std_capabilities/WaypointRunner" x="0.0" y="0.1" />
+        <Event name="std_capabilities/WaypointRunner" provider="std_capabilities/WaypointRunner" x="1.5" y="1.1" />
     </Control>
 </Plan>

capabilities2_fabric/src/capabilities_fabric.cpp

@@ -1,6 +1,6 @@
-#include <capabilities2_executor/capabilities_fabric.hpp>
+#include <capabilities2_fabric/capabilities_fabric.hpp>
 
-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();

capabilities2_fabric/src/capabilities_file_parser.cpp

@@ -1,4 +1,4 @@
-#include <capabilities2_executor/capabilities_file_parser.hpp>
+#include <capabilities2_fabric/capabilities_file_parser.hpp>
 
 int main(int argc, char* argv[])
 {