Merge pull request #28 from CoreSenseEU/adaptation_cascade

Adpatation to work with Cascade LN

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

@@ -47,10 +47,6 @@ class MoveToPredefined : public manipulation::BtActionNode<manipulation_interfac
   }
 
 private:
-  // rclcpp::Node::SharedPtr node_;
-  //  rclcpp::ActionClient<audio_common_msgs::action::TTS>::SharedPtr
-  //  tts_action_;
-
   std::string pose_;
 };
 

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

@@ -45,13 +45,6 @@ class PickObject : public manipulation::BtActionNode<manipulation_interfaces::ac
       {BT::InputPort<moveit_msgs::msg::CollisionObject::SharedPtr>(
           "object_to_pick")});
   }
-
-private:
-  // rclcpp::Node::SharedPtr node_;
-  //  rclcpp::ActionClient<audio_common_msgs::action::TTS>::SharedPtr
-  //  tts_action_;
-
-  // std::string text_;
 };
 
 } // namespace manipulation

bt_nodes/bt_test/CMakeLists.txt

@@ -9,6 +9,7 @@ set(CMAKE_BUILD_TYPE Debug)
 
 find_package(ament_cmake REQUIRED)
 find_package(rclcpp REQUIRED)
+find_package(rclcpp_cascade_lifecycle REQUIRED)
 find_package(rclcpp_action REQUIRED)
 find_package(behaviortree_cpp_v3 REQUIRED)
 find_package(geometry_msgs REQUIRED)
@@ -27,6 +28,7 @@ set(CMAKE_CXX_STANDARD 17)
 
 set(dependencies
   rclcpp
+  rclcpp_cascade_lifecycle
   rclcpp_action
   behaviortree_cpp_v3
   audio_common_msgs

bt_nodes/bt_test/package.xml

@@ -11,6 +11,7 @@
   <buildtool_depend>ament_cmake</buildtool_depend>
 
   <depend>rclcpp</depend>
+  <depend>rclcpp_cascade_lifecycle</depend>
   <depend>rclcpp_action</depend>
   <depend>behaviortree_cpp_v3</depend>
   <depend>geometry_msgs</depend>

bt_nodes/bt_test/src/ask_test.cpp

@@ -6,11 +6,13 @@
 #include "ament_index_cpp/get_package_share_directory.hpp"
 
 #include "rclcpp/rclcpp.hpp"
+#include "rclcpp_cascade_lifecycle/rclcpp_cascade_lifecycle.hpp"
 
 int main(int argc, char * argv[])
 {
   rclcpp::init(argc, argv);
-  auto node = rclcpp::Node::make_shared("ask_test_node");
+  auto node =  std::make_shared<rclcpp_cascade_lifecycle::CascadeLifecycleNode>(
+    "ask_test_node");
   BT::BehaviorTreeFactory factory;
   BT::SharedLibrary loader;
 
@@ -27,16 +29,20 @@ int main(int argc, char * argv[])
 
   auto publisher_zmq = std::make_shared<BT::PublisherZMQ>(tree, 10, 2666, 2667);
 
+  node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE);
+  node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE);
+
   rclcpp::Rate rate(10);
 
   BT::NodeStatus status = BT::NodeStatus::RUNNING;
   bool finish = false;
   while (!finish && rclcpp::ok()) {
+    rclcpp::spin_some(node->get_node_base_interface());
+
     status = tree.rootNode()->executeTick();
     finish = (status == BT::NodeStatus::SUCCESS) ||
       (status == BT::NodeStatus::FAILURE);
-
-    rclcpp::spin_some(node);
+
     rate.sleep();
   }
   std::cout << "Ask Test Finished with status: " << status << std::endl;

bt_nodes/bt_test/src/carry_my_luggage_test.cpp

@@ -23,6 +23,7 @@
 #include "ament_index_cpp/get_package_share_directory.hpp"
 
 #include "rclcpp/rclcpp.hpp"
+#include "rclcpp_cascade_lifecycle/rclcpp_cascade_lifecycle.hpp"
 
 
 int main(int argc, char * argv[])
@@ -32,7 +33,8 @@ int main(int argc, char * argv[])
   rclcpp::NodeOptions options;
   // options.automatically_declare_parameters_from_overrides(true);
 
-  auto node = rclcpp::Node::make_shared("carry_my_luggage", options);
+  auto node = std::make_shared<rclcpp_cascade_lifecycle::CascadeLifecycleNode>(
+    "carry_my_luggage", options);
 
   BT::BehaviorTreeFactory factory;
   BT::SharedLibrary loader;
@@ -57,13 +59,17 @@ int main(int argc, char * argv[])
 
   auto publisher_zmq = std::make_shared<BT::PublisherZMQ>(tree, 10, 2666, 2667);
 
+  node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE);
+  node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE);
+
   rclcpp::Rate rate(10);
 
   bool finish = false;
   while (!finish && rclcpp::ok()) {
+    rclcpp::spin_some(node->get_node_base_interface());
+
     finish = tree.rootNode()->executeTick() != BT::NodeStatus::RUNNING;
 
-    rclcpp::spin_some(node);
     rate.sleep();
   }
 

bt_nodes/bt_test/src/extract_object_from_scene_test.cpp

@@ -23,13 +23,15 @@
 #include "ament_index_cpp/get_package_share_directory.hpp"
 
 #include "rclcpp/rclcpp.hpp"
+#include "rclcpp_cascade_lifecycle/rclcpp_cascade_lifecycle.hpp"
 
 
 int main(int argc, char * argv[])
 {
   rclcpp::init(argc, argv);
 
-  auto node = rclcpp::Node::make_shared("extract_object_from_scene_test_node");
+  auto node = std::make_shared<rclcpp_cascade_lifecycle::CascadeLifecycleNode>(
+    "extract_object_from_scene_test_node");
 
   BT::BehaviorTreeFactory factory;
   BT::SharedLibrary loader;
@@ -47,13 +49,17 @@ int main(int argc, char * argv[])
 
   auto publisher_zmq = std::make_shared<BT::PublisherZMQ>(tree, 10, 1666, 1667);
 
+  node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE);
+  node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE);
+
   rclcpp::Rate rate(10);
 
   bool finish = false;
   while (!finish && rclcpp::ok()) {
+    rclcpp::spin_some(node->get_node_base_interface());
+
     finish = tree.rootNode()->executeTick() != BT::NodeStatus::RUNNING;
 
-    rclcpp::spin_some(node);
     rate.sleep();
   }
 

bt_nodes/bt_test/src/follow_person_test.cpp

@@ -23,13 +23,14 @@
 #include "ament_index_cpp/get_package_share_directory.hpp"
 
 #include "rclcpp/rclcpp.hpp"
-#include "perception_system/PerceptionListener.hpp"
+#include "rclcpp_cascade_lifecycle/rclcpp_cascade_lifecycle.hpp"
 
 int main(int argc, char * argv[])
 {
   rclcpp::init(argc, argv);
 
-  auto node = rclcpp::Node::make_shared("follow_person_test_node");
+  auto node = std::make_shared<rclcpp_cascade_lifecycle::CascadeLifecycleNode>(
+    "follow_person_test_node");
 
   BT::BehaviorTreeFactory factory;
   BT::SharedLibrary loader;
@@ -46,12 +47,15 @@ int main(int argc, char * argv[])
 
   auto publisher_zmq = std::make_shared<BT::PublisherZMQ>(tree, 10, 1666, 1667);
 
+  node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE);
+  node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE);
+
   rclcpp::Rate rate(10);
 
   bool finish = false;
   while (!finish && rclcpp::ok()) {
+    rclcpp::spin_some(node->get_node_base_interface());
     finish = tree.rootNode()->executeTick() != BT::NodeStatus::RUNNING;
-    rclcpp::spin_some(node);
     rate.sleep();
   }
 

bt_nodes/bt_test/src/follow_test.cpp

@@ -26,13 +26,15 @@
 #include "tf2_ros/static_transform_broadcaster.h"
 
 #include "rclcpp/rclcpp.hpp"
+#include "rclcpp_cascade_lifecycle/rclcpp_cascade_lifecycle.hpp"
 
 
 int main(int argc, char * argv[])
 {
   rclcpp::init(argc, argv);
 
-  auto node = rclcpp::Node::make_shared("isdetected_test");
+  auto node = std::make_shared<rclcpp_cascade_lifecycle::CascadeLifecycleNode>(
+    "isdetected_test");
 
   BT::BehaviorTreeFactory factory;
   BT::SharedLibrary loader;
@@ -51,13 +53,17 @@ int main(int argc, char * argv[])
 
   // auto publisher_zmq = std::make_shared<BT::PublisherZMQ>(tree, 10, 1666, 1667);
 
+  node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE);
+  node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE);
+
   rclcpp::Rate rate(10);
 
   bool finish = false;
   while (!finish && rclcpp::ok()) {
+    rclcpp::spin_some(node->get_node_base_interface());
+
     finish = tree.rootNode()->executeTick() != BT::NodeStatus::RUNNING;
 
-    rclcpp::spin_some(node);
     rate.sleep();
   }
 

bt_nodes/bt_test/src/generate_text_from_objects_test.cpp

@@ -23,6 +23,7 @@
 #include "ament_index_cpp/get_package_share_directory.hpp"
 
 #include "rclcpp/rclcpp.hpp"
+#include "rclcpp_cascade_lifecycle/rclcpp_cascade_lifecycle.hpp"
 
 class SayMessage : public BT::SyncActionNode
 {
@@ -47,7 +48,8 @@ int main(int argc, char * argv[])
 {
   rclcpp::init(argc, argv);
 
-  auto node = rclcpp::Node::make_shared("generate_text_from_objects_test_node");
+  auto node = std::make_shared<rclcpp_cascade_lifecycle::CascadeLifecycleNode>(
+    "generate_text_from_objects_test_node");
 
   BT::BehaviorTreeFactory factory;
   BT::SharedLibrary loader;
@@ -69,13 +71,17 @@ int main(int argc, char * argv[])
 
   auto publisher_zmq = std::make_shared<BT::PublisherZMQ>(tree, 10, 1666, 1667);
 
+  node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE);
+  node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE);
+
   rclcpp::Rate rate(10);
 
   bool finish = false;
   while (!finish && rclcpp::ok()) {
+    rclcpp::spin_some(node->get_node_base_interface());
+
     finish = tree.rootNode()->executeTick() != BT::NodeStatus::RUNNING;
 
-    rclcpp::spin_some(node);
     rate.sleep();
   }
 

bt_nodes/bt_test/src/init_carry_test.cpp

@@ -23,16 +23,15 @@
 #include "ament_index_cpp/get_package_share_directory.hpp"
 
 #include "rclcpp/rclcpp.hpp"
+#include "rclcpp_cascade_lifecycle/rclcpp_cascade_lifecycle.hpp"
 
 
 int main(int argc, char * argv[])
 {
   rclcpp::init(argc, argv);
 
-  rclcpp::NodeOptions options;
-  options.automatically_declare_parameters_from_overrides(true);
-
-  auto node = rclcpp::Node::make_shared("init_carry_test", options);
+  auto node = std::make_shared<rclcpp_cascade_lifecycle::CascadeLifecycleNode>(
+    "init_carry_test");
 
   BT::BehaviorTreeFactory factory;
   BT::SharedLibrary loader;
@@ -48,13 +47,17 @@ int main(int argc, char * argv[])
 
   auto publisher_zmq = std::make_shared<BT::PublisherZMQ>(tree, 10, 1666, 1667);
 
+  node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE);
+  node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE);
+
   rclcpp::Rate rate(10);
 
   bool finish = false;
   while (!finish && rclcpp::ok()) {
+    rclcpp::spin_some(node->get_node_base_interface());
+
     finish = tree.rootNode()->executeTick() != BT::NodeStatus::RUNNING;
 
-    rclcpp::spin_some(node);
     rate.sleep();
   }
 

bt_nodes/bt_test/src/is_detected_moveto_loop_test.cpp

@@ -26,13 +26,15 @@
 #include "tf2_ros/static_transform_broadcaster.h"
 
 #include "rclcpp/rclcpp.hpp"
+#include "rclcpp_cascade_lifecycle/rclcpp_cascade_lifecycle.hpp"
 
 
 int main(int argc, char * argv[])
 {
   rclcpp::init(argc, argv);
 
-  auto node = rclcpp::Node::make_shared("loop_isdetected_moveto_test");
+  auto node = std::make_shared<rclcpp_cascade_lifecycle::CascadeLifecycleNode>(
+    "loop_isdetected_moveto_test");
 
   BT::BehaviorTreeFactory factory;
   BT::SharedLibrary loader;
@@ -50,13 +52,17 @@ int main(int argc, char * argv[])
 
   auto publisher_zmq = std::make_shared<BT::PublisherZMQ>(tree, 10, 1666, 1667);
 
+  node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE);
+  node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE);
+
   rclcpp::Rate rate(10);
 
   bool finish = false;
   while (!finish && rclcpp::ok()) {
+    rclcpp::spin_some(node->get_node_base_interface());
+
     finish = tree.rootNode()->executeTick() != BT::NodeStatus::RUNNING;
 
-    rclcpp::spin_some(node);
     rate.sleep();
   }
 

bt_nodes/bt_test/src/is_detected_real_test.cpp

@@ -26,13 +26,15 @@
 #include "tf2_ros/static_transform_broadcaster.h"
 
 #include "rclcpp/rclcpp.hpp"
+#include "rclcpp_cascade_lifecycle/rclcpp_cascade_lifecycle.hpp"
 
 
 int main(int argc, char * argv[])
 {
   rclcpp::init(argc, argv);
 
-  auto node = rclcpp::Node::make_shared("isdetected_test");
+  auto node = std::make_shared<rclcpp_cascade_lifecycle::CascadeLifecycleNode>(
+    "isdetected_test");
 
   BT::BehaviorTreeFactory factory;
   BT::SharedLibrary loader;
@@ -49,13 +51,17 @@ int main(int argc, char * argv[])
 
   auto publisher_zmq = std::make_shared<BT::PublisherZMQ>(tree, 10, 1666, 1667);
 
+  node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE);
+  node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE);
+
   rclcpp::Rate rate(10);
 
   bool finish = false;
   while (!finish && rclcpp::ok()) {
+    rclcpp::spin_some(node->get_node_base_interface());
+
     finish = tree.rootNode()->executeTick() != BT::NodeStatus::RUNNING;
 
-    rclcpp::spin_some(node);
     rate.sleep();
   }