Merge pull request #30 from CoreSenseEU/behaviors_program

Behaviors program

Author: Juancams

bt_nodes/arm/CMakeLists.txt

@@ -9,6 +9,7 @@ endif()
 find_package(ament_cmake REQUIRED)
 find_package(behaviortree_cpp_v3 REQUIRED)
 find_package(rclcpp REQUIRED)
+find_package(rclcpp_cascade_lifecycle REQUIRED)
 find_package(rclcpp_action REQUIRED)
 find_package(manipulation_interfaces REQUIRED)
 # uncomment the following section in order to fill in
@@ -20,6 +21,7 @@ set(CMAKE_CXX_STANDARD 17)
 set(dependencies
   behaviortree_cpp_v3
   rclcpp
+  rclcpp_cascade_lifecycle
   rclcpp_action
   manipulation_interfaces
 )

bt_nodes/arm/include/arm/manipulation/move_to_predefined.hpp

@@ -23,12 +23,16 @@
 #include "behaviortree_cpp_v3/behavior_tree.h"
 #include "behaviortree_cpp_v3/bt_factory.h"
 #include "arm/manipulation/BTActionNode.hpp"
+
 #include "rclcpp/rclcpp.hpp"
+#include "rclcpp_cascade_lifecycle/rclcpp_cascade_lifecycle.hpp"
 
 namespace manipulation
 {
 
-class MoveToPredefined : public manipulation::BtActionNode<manipulation_interfaces::action::MoveToPredefined>
+class MoveToPredefined : public manipulation::BtActionNode<
+    manipulation_interfaces::action::MoveToPredefined,
+    rclcpp_cascade_lifecycle::CascadeLifecycleNode>
 {
 public:
   explicit MoveToPredefined(

bt_nodes/arm/include/arm/manipulation/pick_object.hpp

@@ -23,12 +23,15 @@
 #include "behaviortree_cpp_v3/behavior_tree.h"
 #include "behaviortree_cpp_v3/bt_factory.h"
 #include "arm/manipulation/BTActionNode.hpp"
+
 #include "rclcpp/rclcpp.hpp"
+#include "rclcpp_cascade_lifecycle/rclcpp_cascade_lifecycle.hpp"
 
 namespace manipulation
 {
 
-class PickObject : public manipulation::BtActionNode<manipulation_interfaces::action::Pick>
+class PickObject : public manipulation::BtActionNode<
+    manipulation_interfaces::action::Pick, rclcpp_cascade_lifecycle::CascadeLifecycleNode>
 {
 public:
   explicit PickObject(

bt_nodes/arm/package.xml

@@ -11,6 +11,7 @@
   <depend>behaviortree_cpp_v3</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_action</depend>
+  <depend>rclcpp_cascade_lifecycle</depend>
   <depend>manipulation_interfaces</depend>
 
   <test_depend>ament_lint_auto</test_depend>

bt_nodes/arm/src/manipulation/move_to_predefined.cpp

@@ -28,8 +28,10 @@ using namespace std::placeholders;
 MoveToPredefined::MoveToPredefined(
   const std::string & xml_tag_name, const std::string & action_name,
   const BT::NodeConfiguration & conf)
-: manipulation::BtActionNode<manipulation_interfaces::action::MoveToPredefined>(xml_tag_name,
-    action_name, conf) {}
+: manipulation::BtActionNode<
+    manipulation_interfaces::action::MoveToPredefined,
+    rclcpp_cascade_lifecycle::CascadeLifecycleNode>(
+    xml_tag_name, action_name, conf) {}
 
 void MoveToPredefined::on_tick()
 {

bt_nodes/arm/src/manipulation/pick_object.cpp

@@ -29,8 +29,9 @@ using namespace std::placeholders;
 PickObject::PickObject(
   const std::string & xml_tag_name, const std::string & action_name,
   const BT::NodeConfiguration & conf)
-: manipulation::BtActionNode<manipulation_interfaces::action::Pick>(xml_tag_name,
-    action_name, conf) {}
+: manipulation::BtActionNode<
+    manipulation_interfaces::action::Pick, rclcpp_cascade_lifecycle::CascadeLifecycleNode>(
+    xml_tag_name, action_name, conf) {}
 
 void PickObject::on_tick()
 {

bt_nodes/bt_test/src/ask_test.cpp

@@ -11,7 +11,7 @@
 int main(int argc, char * argv[])
 {
   rclcpp::init(argc, argv);
-  auto node =  std::make_shared<rclcpp_cascade_lifecycle::CascadeLifecycleNode>(
+  auto node = std::make_shared<rclcpp_cascade_lifecycle::CascadeLifecycleNode>(
     "ask_test_node");
   BT::BehaviorTreeFactory factory;
   BT::SharedLibrary loader;
@@ -42,7 +42,7 @@ int main(int argc, char * argv[])
     status = tree.rootNode()->executeTick();
     finish = (status == BT::NodeStatus::SUCCESS) ||
       (status == BT::NodeStatus::FAILURE);
-    
+
     rate.sleep();
   }
   std::cout << "Ask Test Finished with status: " << status << std::endl;

bt_nodes/bt_test/src/look_at_test.cpp

@@ -57,7 +57,7 @@ int main(int argc, char * argv[])
   bool finish = false;
   while (!finish && rclcpp::ok()) {
     rclcpp::spin_some(node->get_node_base_interface());
- 
+
     finish = tree.rootNode()->executeTick() != BT::NodeStatus::SUCCESS;
 
     rate.sleep();

bt_nodes/configuration/include/configuration/SetWp.hpp

@@ -43,7 +43,7 @@ class SetWp : public BT::ActionNodeBase
   }
 
 private:
-   std::shared_ptr<rclcpp_cascade_lifecycle::CascadeLifecycleNode> node_;
+  std::shared_ptr<rclcpp_cascade_lifecycle::CascadeLifecycleNode> node_;
   std::vector<std::string> wp_names_;
 };
 

bt_nodes/configuration/include/configuration/init_carry.hpp

@@ -44,7 +44,7 @@ class InitCarry : public BT::ActionNodeBase
   }
 
 private:
-   std::shared_ptr<rclcpp_cascade_lifecycle::CascadeLifecycleNode> node_;
+  std::shared_ptr<rclcpp_cascade_lifecycle::CascadeLifecycleNode> node_;
   std::string cam_frame_, home_pose_, offer_pose_;
   int person_id;
   geometry_msgs::msg::PoseStamped home_position_;