Merge pull request #30 from CoreSenseEU/behaviors_program

Behaviors program
CoreSenseEU · Apr 25, 2024 · f9de8f5 · f9de8f5
2 parents 062e55a + af7706c
commit f9de8f5
Show file tree

Hide file tree

Showing 52 changed files with 1,344 additions and 56 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,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(

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,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()
 {

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/bt_test/src/ask_test.cpp b/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;

diff --git a/bt_nodes/bt_test/src/look_at_test.cpp b/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();

diff --git a/bt_nodes/configuration/include/configuration/SetWp.hpp b/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_;
 };
 

diff --git a/bt_nodes/configuration/include/configuration/init_carry.hpp b/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_;

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,9 @@ 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/ctrl_support/BTActionNode.hpp b/bt_nodes/motion/include/ctrl_support/BTActionNode.hpp
@@ -235,7 +235,8 @@ class BtActionNode : public BT::ActionNodeBase
       };
 
     send_goal_options.feedback_callback =
-      std::bind(&BtActionNode::feedback_callback, this,
+      std::bind(
+      &BtActionNode::feedback_callback, this,
       std::placeholders::_1, std::placeholders::_2);
 
     auto future_goal_handle = action_client_->async_send_goal(goal_, send_goal_options);
@@ -252,7 +253,7 @@ class BtActionNode : public BT::ActionNodeBase
       throw std::runtime_error("Goal was rejected by the action server");
     }
   }
-  
+
   void feedback_callback(
     typename rclcpp_action::ClientGoalHandle<ActionT>::SharedPtr,
     const typename ActionT::Feedback::ConstSharedPtr feedback)
@@ -263,7 +264,7 @@ class BtActionNode : public BT::ActionNodeBase
 
   virtual void on_feedback()
   {
-    
+
   }
 
   void increment_recovery_count()

diff --git a/bt_nodes/motion/include/motion/head/LookAt.hpp b/bt_nodes/motion/include/motion/head/LookAt.hpp
@@ -65,4 +65,4 @@ class LookAt : public BT::ActionNodeBase
 
 }  // namespace navigation
 
-#endif  // NAVIGATION__LOOK_AT_HPP_
+#endif  // NAVIGATION__LOOK_AT_HPP_
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(
@@ -41,7 +44,7 @@ class Pan : public motion::BtActionNode<control_msgs::action::FollowJointTraject
   BT::NodeStatus on_success() override;
   BT::NodeStatus on_aborted() override;
   BT::NodeStatus on_cancelled() override;
-  
+
 
   static BT::PortsList providedPorts()
   {
@@ -50,6 +53,7 @@ class Pan : public motion::BtActionNode<control_msgs::action::FollowJointTraject
         BT::InputPort<std::string>("tf_frame"),
       });
   }
+
 private:
   rclcpp::Node::SharedPtr node_;
   BT::Optional<std::string> point_to_pan_;

diff --git a/bt_nodes/motion/include/motion/navigation/MoveTo.hpp b/bt_nodes/motion/include/motion/navigation/MoveTo.hpp
@@ -40,7 +40,8 @@ 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/LookAt.cpp b/bt_nodes/motion/src/motion/head/LookAt.cpp
@@ -66,4 +66,4 @@ void LookAt::halt() {RCLCPP_INFO(node_->get_logger(), "LookAt halted");}
 
 BT_REGISTER_NODES(factory) {
   factory.registerNodeType<head::LookAt>("LookAt");
-}
+}