diff --git a/bt_nodes/arm/CMakeLists.txt b/bt_nodes/arm/CMakeLists.txt index d4aa6ec..cb2cd8f 100644 --- 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 index 065b35b..e6f31a0 100644 --- 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 +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 index ac6fc15..3df449c 100644 --- 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 +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 index c4ba866..c5e653b 100644 --- a/bt_nodes/arm/package.xml +++ b/bt_nodes/arm/package.xml @@ -11,6 +11,7 @@ behaviortree_cpp_v3 rclcpp rclcpp_action + rclcpp_cascade_lifecycle manipulation_interfaces ament_lint_auto diff --git a/bt_nodes/arm/src/manipulation/move_to_predefined.cpp b/bt_nodes/arm/src/manipulation/move_to_predefined.cpp index a2a3033..c582097 100644 --- 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(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 index 430190b..4275063 100644 --- 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(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 index a434f14..9b8a07d 100644 --- 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("node"); + config().blackboard->get("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 index d0f0a63..be9b097 100644 --- a/bt_nodes/hri/include/hri/dialog/DialogConfirmation.hpp +++ b/bt_nodes/hri/include/hri/dialog/DialogConfirmation.hpp @@ -21,7 +21,10 @@ #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" @@ -29,7 +32,8 @@ namespace dialog { class DialogConfirmation - : public dialog::BtActionNode + : 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 index ffbfe10..0af3737 100644 --- 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 +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 index d4196df..cec8795 100644 --- 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 + : 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 index 7e40966..5578a81 100644 --- 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 +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 } private: - std::shared_ptr node_; - rclcpp_lifecycle::LifecyclePublisher::SharedPtr publisher_; rclcpp_lifecycle::LifecyclePublisher::SharedPtr publisher_start_; diff --git a/bt_nodes/hri/src/hri/dialog/DialogConfirmation.cpp b/bt_nodes/hri/src/hri/dialog/DialogConfirmation.cpp index e163c80..831cafb 100644 --- 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(xml_tag_name, action_name, - conf) +: dialog::BtActionNode( + xml_tag_name, action_name, conf) { - this->publisher_start_ = - node_->create_publisher("dialog_action", 10); + publisher_start_ = node_->create_publisher("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 index d8f74f5..45f9ebc 100644 --- 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(xml_tag_name, action_name, - conf) +: dialog::BtActionNode( + xml_tag_name, action_name,conf) { - this->publisher_start_ = - node_->create_publisher("dialog_action", 10); + publisher_start_ = node_->create_publisher("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 index 068ecaa..2274951 100644 --- 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( +: dialog::BtActionNode< + llama_msgs::action::GenerateResponse, rclcpp_cascade_lifecycle::CascadeLifecycleNode>( xml_tag_name, action_name, conf) { - this->publisher_start_ = - node_->create_publisher("dialog_action", 10); + publisher_start_ = node_->create_publisher("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 index fab63bd..4c5a495 100644 --- 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(xml_tag_name, - action_name, conf) +: dialog::BtActionNode( + 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 index 00ef065..13a6322 100644 --- 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 +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 index 24806f7..3536066 100644 --- 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 + : public motion::BtActionNode { public: explicit MoveTo( diff --git a/bt_nodes/motion/src/motion/head/Pan.cpp b/bt_nodes/motion/src/motion/head/Pan.cpp index 092aefc..bc84358 100644 --- 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(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 index 474726c..33b71ab 100644 --- 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(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_) {