Merge pull request #8 from CollaborativeRoboticsLab/remove-container

Remove container

Author: KalanaRatnayake

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_fabric.hpp renamed to capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric.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>
 
 /**

capabilities2_fabric/include/capabilities2_executor/capabilities_file_parser.hpp renamed to capabilities2_fabric/include/capabilities2_fabric/capabilities_file_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
 {

capabilities2_fabric/include/capabilities2_executor/capabilities_xml_parser.hpp renamed to capabilities2_fabric/include/capabilities2_fabric/capabilities_xml_parser.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/include/capabilities2_fabric/utils/bond_client.hpp

@@ -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/include/capabilities2_executor/structs/connection.hpp renamed to capabilities2_fabric/include/capabilities2_fabric/utils/connection.hpp

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