Adapr BT nodes to Cascade LN

Signed-off-by: Francisco Martín Rico <[email protected]>
CoreSenseEU · Apr 25, 2024 · 0a15d6a · 0a15d6a
1 parent 959016d
commit 0a15d6a
Show file tree

Hide file tree

Showing 19 changed files with 60 additions and 34 deletions.
diff --git a/bt_nodes/arm/CMakeLists.txt b/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
 )

diff --git a/bt_nodes/arm/include/arm/manipulation/move_to_predefined.hpp b/bt_nodes/arm/include/arm/manipulation/move_to_predefined.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 MoveToPredefined : public manipulation::BtActionNode<manipulation_interfaces::action::MoveToPredefined>
+class MoveToPredefined : public manipulation::BtActionNode<
+  manipulation_interfaces::action::MoveToPredefined, rclcpp_cascade_lifecycle::CascadeLifecycleNode>
 {
 public:
   explicit MoveToPredefined(

diff --git a/bt_nodes/arm/include/arm/manipulation/pick_object.hpp b/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(

diff --git a/bt_nodes/arm/package.xml b/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>

diff --git a/bt_nodes/arm/src/manipulation/move_to_predefined.cpp b/bt_nodes/arm/src/manipulation/move_to_predefined.cpp
@@ -28,8 +28,9 @@ 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()
 {

diff --git a/bt_nodes/arm/src/manipulation/pick_object.cpp b/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()
 {

diff --git a/bt_nodes/hri/include/hri/dialog/BTActionNode.hpp b/bt_nodes/hri/include/hri/dialog/BTActionNode.hpp
@@ -36,7 +36,7 @@ class BtActionNode : public BT::ActionNodeBase
     const BT::NodeConfiguration & conf)
   : BT::ActionNodeBase(xml_tag_name, conf), action_name_(action_name)
   {
-    node_ = config().blackboard->get<typename NodeT::SharedPtr>("node");
+    config().blackboard->get<typename NodeT::SharedPtr>("node", node_);
 
     server_timeout_ = 1s;
 

diff --git a/bt_nodes/hri/include/hri/dialog/DialogConfirmation.hpp b/bt_nodes/hri/include/hri/dialog/DialogConfirmation.hpp
@@ -21,15 +21,19 @@
 #include "behaviortree_cpp_v3/behavior_tree.h"
 #include "behaviortree_cpp_v3/bt_factory.h"
 #include "hri/dialog/BTActionNode.hpp"
+
 #include "rclcpp/rclcpp.hpp"
+#include "rclcpp_cascade_lifecycle/rclcpp_cascade_lifecycle.hpp"
+
 #include "std_msgs/msg/int8.hpp"
 #include "whisper_msgs/action/stt.hpp"
 
 namespace dialog
 {
 
 class DialogConfirmation
-  : public dialog::BtActionNode<whisper_msgs::action::STT>
+  : public dialog::BtActionNode<
+  whisper_msgs::action::STT, rclcpp_cascade_lifecycle::CascadeLifecycleNode>
 {
 public:
   explicit DialogConfirmation(

diff --git a/bt_nodes/hri/include/hri/dialog/Listen.hpp b/bt_nodes/hri/include/hri/dialog/Listen.hpp
@@ -22,14 +22,18 @@
 #include "behaviortree_cpp_v3/behavior_tree.h"
 #include "behaviortree_cpp_v3/bt_factory.h"
 #include "hri/dialog/BTActionNode.hpp"
+
 #include "rclcpp/rclcpp.hpp"
+#include "rclcpp_cascade_lifecycle/rclcpp_cascade_lifecycle.hpp"
+
 #include "std_msgs/msg/int8.hpp"
 #include "whisper_msgs/action/stt.hpp"
 
 namespace dialog
 {
 
-class Listen : public dialog::BtActionNode<whisper_msgs::action::STT>
+class Listen : public dialog::BtActionNode<
+  whisper_msgs::action::STT, rclcpp_cascade_lifecycle::CascadeLifecycleNode>
 {
 public:
   explicit Listen(

diff --git a/bt_nodes/hri/include/hri/dialog/Query.hpp b/bt_nodes/hri/include/hri/dialog/Query.hpp
@@ -22,14 +22,18 @@
 #include "behaviortree_cpp_v3/bt_factory.h"
 #include "hri/dialog/BTActionNode.hpp"
 #include "llama_msgs/action/generate_response.hpp"
+
 #include "rclcpp/rclcpp.hpp"
+#include "rclcpp_cascade_lifecycle/rclcpp_cascade_lifecycle.hpp"
+
 #include "std_msgs/msg/int8.hpp"
 
 namespace dialog
 {
 
 class Query
-  : public dialog::BtActionNode<llama_msgs::action::GenerateResponse>
+  : public dialog::BtActionNode<
+  llama_msgs::action::GenerateResponse, rclcpp_cascade_lifecycle::CascadeLifecycleNode>
 {
 public:
   explicit Query(

diff --git a/bt_nodes/hri/include/hri/dialog/Speak.hpp b/bt_nodes/hri/include/hri/dialog/Speak.hpp
@@ -34,7 +34,8 @@
 namespace dialog
 {
 
-class Speak : public dialog::BtActionNode<audio_common_msgs::action::TTS>
+class Speak : public dialog::BtActionNode<
+  audio_common_msgs::action::TTS, rclcpp_cascade_lifecycle::CascadeLifecycleNode>
 {
 public:
   explicit Speak(
@@ -53,8 +54,6 @@ class Speak : public dialog::BtActionNode<audio_common_msgs::action::TTS>
   }
 
 private:
-  std::shared_ptr<rclcpp_cascade_lifecycle::CascadeLifecycleNode> node_;
-
   rclcpp_lifecycle::LifecyclePublisher<std_msgs::msg::String>::SharedPtr publisher_;
   rclcpp_lifecycle::LifecyclePublisher<std_msgs::msg::Int8>::SharedPtr publisher_start_;
 

diff --git a/bt_nodes/hri/src/hri/dialog/DialogConfirmation.cpp b/bt_nodes/hri/src/hri/dialog/DialogConfirmation.cpp
@@ -31,11 +31,10 @@ DialogConfirmation::DialogConfirmation(
   const std::string & xml_tag_name,
   const std::string & action_name,
   const BT::NodeConfiguration & conf)
-: dialog::BtActionNode<whisper_msgs::action::STT>(xml_tag_name, action_name,
-    conf)
+: dialog::BtActionNode<whisper_msgs::action::STT, rclcpp_cascade_lifecycle::CascadeLifecycleNode>(
+    xml_tag_name, action_name, conf)
 {
-  this->publisher_start_ =
-    node_->create_publisher<std_msgs::msg::Int8>("dialog_action", 10);
+  publisher_start_ = node_->create_publisher<std_msgs::msg::Int8>("dialog_action", 10);
 }
 
 void DialogConfirmation::on_tick()
@@ -49,7 +48,7 @@ void DialogConfirmation::on_tick()
 
   msg_dialog_action.data = 0;
 
-  this->publisher_start_->publish(msg_dialog_action);
+  publisher_start_->publish(msg_dialog_action);
 
   // goal_.text = text_;
 }

diff --git a/bt_nodes/hri/src/hri/dialog/Listen.cpp b/bt_nodes/hri/src/hri/dialog/Listen.cpp
@@ -31,11 +31,10 @@ using namespace std::placeholders;
 Listen::Listen(
   const std::string & xml_tag_name, const std::string & action_name,
   const BT::NodeConfiguration & conf)
-: dialog::BtActionNode<whisper_msgs::action::STT>(xml_tag_name, action_name,
-    conf)
+: dialog::BtActionNode<whisper_msgs::action::STT, rclcpp_cascade_lifecycle::CascadeLifecycleNode>(
+    xml_tag_name, action_name,conf)
 {
-  this->publisher_start_ =
-    node_->create_publisher<std_msgs::msg::Int8>("dialog_action", 10);
+  publisher_start_ = node_->create_publisher<std_msgs::msg::Int8>("dialog_action", 10);
 }
 
 void Listen::on_tick()
@@ -48,7 +47,7 @@ void Listen::on_tick()
 
   msg_dialog_action.data = 0;
 
-  this->publisher_start_->publish(msg_dialog_action);
+  publisher_start_->publish(msg_dialog_action);
 }
 
 BT::NodeStatus Listen::on_success()

diff --git a/bt_nodes/hri/src/hri/dialog/Query.cpp b/bt_nodes/hri/src/hri/dialog/Query.cpp
@@ -33,11 +33,11 @@ using json = nlohmann::json;
 Query::Query(
   const std::string & xml_tag_name, const std::string & action_name,
   const BT::NodeConfiguration & conf)
-: dialog::BtActionNode<llama_msgs::action::GenerateResponse>(
+: dialog::BtActionNode<
+  llama_msgs::action::GenerateResponse, rclcpp_cascade_lifecycle::CascadeLifecycleNode>(
     xml_tag_name, action_name, conf)
 {
-  this->publisher_start_ =
-    node_->create_publisher<std_msgs::msg::Int8>("dialog_action", 10);
+  publisher_start_ = node_->create_publisher<std_msgs::msg::Int8>("dialog_action", 10);
 }
 
 void Query::on_tick()
@@ -86,7 +86,7 @@ ws ::= ([ \t\n] ws)?)";
 
   msg_dialog_action.data = 2;
 
-  this->publisher_start_->publish(msg_dialog_action);
+  publisher_start_->publish(msg_dialog_action);
 }
 
 BT::NodeStatus Query::on_success()

diff --git a/bt_nodes/hri/src/hri/dialog/Speak.cpp b/bt_nodes/hri/src/hri/dialog/Speak.cpp
@@ -31,8 +31,8 @@ using namespace std::placeholders;
 Speak::Speak(
   const std::string & xml_tag_name, const std::string & action_name,
   const BT::NodeConfiguration & conf)
-: dialog::BtActionNode<audio_common_msgs::action::TTS>(xml_tag_name,
-    action_name, conf)
+: dialog::BtActionNode<audio_common_msgs::action::TTS, rclcpp_cascade_lifecycle::CascadeLifecycleNode>(
+    xml_tag_name, action_name, conf)
 {
   config().blackboard->get("node", node_);
 

diff --git a/bt_nodes/motion/include/motion/head/Pan.hpp b/bt_nodes/motion/include/motion/head/Pan.hpp
@@ -22,12 +22,15 @@
 #include "behaviortree_cpp_v3/bt_factory.h"
 #include "ctrl_support/BTActionNode.hpp"
 #include "control_msgs/action/follow_joint_trajectory.hpp"
+
 #include "rclcpp/rclcpp.hpp"
+#include "rclcpp_cascade_lifecycle/rclcpp_cascade_lifecycle.hpp"
 
 namespace head
 {
 
-class Pan : public motion::BtActionNode<control_msgs::action::FollowJointTrajectory>
+class Pan : public motion::BtActionNode<
+  control_msgs::action::FollowJointTrajectory, rclcpp_cascade_lifecycle::CascadeLifecycleNode>
 {
 public:
   explicit Pan(

diff --git a/bt_nodes/motion/include/motion/navigation/MoveTo.hpp b/bt_nodes/motion/include/motion/navigation/MoveTo.hpp
@@ -40,7 +40,7 @@ namespace navigation
 {
 
 class MoveTo
-  : public motion::BtActionNode<nav2_msgs::action::NavigateToPose>
+  : public motion::BtActionNode<nav2_msgs::action::NavigateToPose, rclcpp_cascade_lifecycle::CascadeLifecycleNode>
 {
 public:
   explicit MoveTo(

diff --git a/bt_nodes/motion/src/motion/head/Pan.cpp b/bt_nodes/motion/src/motion/head/Pan.cpp
@@ -26,7 +26,9 @@ Pan::Pan(
   const std::string & xml_tag_name,
   const std::string & action_name,
   const BT::NodeConfiguration & conf)
-: motion::BtActionNode<control_msgs::action::FollowJointTrajectory>(xml_tag_name, action_name, conf)
+: motion::BtActionNode<
+  control_msgs::action::FollowJointTrajectory, rclcpp_cascade_lifecycle::CascadeLifecycleNode>(
+    xml_tag_name, action_name, conf)
 {
   config().blackboard->get("node", node_);
 }

diff --git a/bt_nodes/motion/src/motion/navigation/MoveTo.cpp b/bt_nodes/motion/src/motion/navigation/MoveTo.cpp
@@ -32,8 +32,9 @@ MoveTo::MoveTo(
   const std::string & xml_tag_name,
   const std::string & action_name,
   const BT::NodeConfiguration & conf)
-: motion::BtActionNode<nav2_msgs::action::NavigateToPose>(xml_tag_name,
-    action_name, conf),
+: motion::BtActionNode<
+  nav2_msgs::action::NavigateToPose, rclcpp_cascade_lifecycle::CascadeLifecycleNode>(
+    xml_tag_name, action_name, conf),
   tf_buffer_(),
   tf_listener_(tf_buffer_)
 {