From 9f0152523abeb2d295417081699cbe094b8d975e Mon Sep 17 00:00:00 2001 From: igonzf Date: Mon, 20 May 2024 16:14:19 +0200 Subject: [PATCH] change Speak from BtActionNode to ActionNodeBase --- .../include/hri/dialog/DialogConfirmation.hpp | 8 +- bt_nodes/hri/include/hri/dialog/Speak.hpp | 15 ++- .../hri/src/hri/dialog/DialogConfirmation.cpp | 11 +- bt_nodes/hri/src/hri/dialog/Speak.cpp | 107 +++++++++++++++--- 4 files changed, 111 insertions(+), 30 deletions(-) diff --git a/bt_nodes/hri/include/hri/dialog/DialogConfirmation.hpp b/bt_nodes/hri/include/hri/dialog/DialogConfirmation.hpp index d182fec..83744cd 100644 --- a/bt_nodes/hri/include/hri/dialog/DialogConfirmation.hpp +++ b/bt_nodes/hri/include/hri/dialog/DialogConfirmation.hpp @@ -24,10 +24,10 @@ #include "rclcpp/rclcpp.hpp" #include "rclcpp_action/rclcpp_action.hpp" -#include "rclcpp_cascade_lifecycle/rclcpp_cascade_lifecycle.hpp" #include "std_msgs/msg/int8.hpp" #include "whisper_msgs/action/stt.hpp" +#include "rclcpp_cascade_lifecycle/rclcpp_cascade_lifecycle.hpp" namespace dialog { @@ -46,10 +46,10 @@ class DialogConfirmation : public BT::ActionNodeBase private: BT::NodeStatus on_idle(); - rclcpp::Node::SharedPtr node_; - rclcpp::Publisher::SharedPtr publisher_start_; + std::shared_ptr node_; + rclcpp_lifecycle::LifecyclePublisher::SharedPtr publisher_start_; std::shared_ptr> client_; - std::string text_; + std::string text_ = ""; bool is_goal_sent_ = false; }; diff --git a/bt_nodes/hri/include/hri/dialog/Speak.hpp b/bt_nodes/hri/include/hri/dialog/Speak.hpp index db1537b..f9a04a2 100644 --- a/bt_nodes/hri/include/hri/dialog/Speak.hpp +++ b/bt_nodes/hri/include/hri/dialog/Speak.hpp @@ -26,6 +26,7 @@ #include "std_msgs/msg/int8.hpp" #include "std_msgs/msg/string.hpp" +#include "audio_common_msgs/action/tts.hpp" #include "rclcpp/rclcpp.hpp" #include "rclcpp_cascade_lifecycle/rclcpp_cascade_lifecycle.hpp" @@ -34,16 +35,15 @@ namespace dialog { -class Speak : public dialog::BtActionNode< - audio_common_msgs::action::TTS, rclcpp_cascade_lifecycle::CascadeLifecycleNode> +class Speak : public BT::ActionNodeBase { public: explicit Speak( - const std::string & xml_tag_name, const std::string & action_name, + const std::string & xml_tag_name, const BT::NodeConfiguration & conf); - void on_tick() override; - BT::NodeStatus on_success() override; + void halt(); + BT::NodeStatus tick(); static BT::PortsList providedPorts() { @@ -52,9 +52,12 @@ class Speak : public dialog::BtActionNode< } private: + BT::NodeStatus on_idle(); + std::shared_ptr node_; rclcpp_lifecycle::LifecyclePublisher::SharedPtr publisher_; rclcpp_lifecycle::LifecyclePublisher::SharedPtr publisher_start_; - + std::shared_ptr> client_; + bool is_goal_sent_ = false; // rclcpp::Node::SharedPtr node_; // rclcpp::ActionClient::SharedPtr // tts_action_; diff --git a/bt_nodes/hri/src/hri/dialog/DialogConfirmation.cpp b/bt_nodes/hri/src/hri/dialog/DialogConfirmation.cpp index 8de1d79..9f7e44e 100644 --- a/bt_nodes/hri/src/hri/dialog/DialogConfirmation.cpp +++ b/bt_nodes/hri/src/hri/dialog/DialogConfirmation.cpp @@ -38,6 +38,7 @@ DialogConfirmation::DialogConfirmation( config().blackboard->get("node", node_); publisher_start_ = node_->create_publisher("dialog_action", 10); + publisher_start_->on_activate(); client_ = rclcpp_action::create_client( node_, "whisper/listen"); } @@ -60,6 +61,10 @@ BT::NodeStatus DialogConfirmation::tick() publisher_start_->publish(msg_dialog_action); // goal_.text = text_; */ + auto msg_dialog_action = std_msgs::msg::Int8(); + msg_dialog_action.data = 0; + publisher_start_->publish(msg_dialog_action); + if (status() == BT::NodeStatus::IDLE || !is_goal_sent_) { return on_idle(); } @@ -79,7 +84,7 @@ BT::NodeStatus DialogConfirmation::tick() } else { return BT::NodeStatus::FAILURE; } - + return BT::NodeStatus::RUNNING; } @@ -87,13 +92,9 @@ BT::NodeStatus DialogConfirmation::on_idle() { auto goal = whisper_msgs::action::STT::Goal(); - auto msg_dialog_action = std_msgs::msg::Int8(); RCLCPP_INFO(node_->get_logger(), "Sending goal"); - msg_dialog_action.data = 0; - publisher_start_->publish(msg_dialog_action); - auto future_goal_handle = client_->async_send_goal(goal); if (rclcpp::spin_until_future_complete( node_->get_node_base_interface(), diff --git a/bt_nodes/hri/src/hri/dialog/Speak.cpp b/bt_nodes/hri/src/hri/dialog/Speak.cpp index 90583bb..0d4a085 100644 --- a/bt_nodes/hri/src/hri/dialog/Speak.cpp +++ b/bt_nodes/hri/src/hri/dialog/Speak.cpp @@ -29,11 +29,9 @@ using namespace std::chrono_literals; using namespace std::placeholders; Speak::Speak( - const std::string & xml_tag_name, const std::string & action_name, + const std::string & xml_tag_name, const BT::NodeConfiguration & conf) -: dialog::BtActionNode( - xml_tag_name, action_name, conf) +: BT::ActionNodeBase(xml_tag_name, conf) { config().blackboard->get("node", node_); @@ -44,12 +42,25 @@ Speak::Speak( this->publisher_->on_activate(); this->publisher_start_->on_activate(); + + this->client_ = rclcpp_action::create_client( + node_, "/say"); +} + +void Speak::halt() +{ + RCLCPP_INFO(node_->get_logger(), "Speak halted"); } -void Speak::on_tick() +BT::NodeStatus Speak::tick() { RCLCPP_DEBUG(node_->get_logger(), "Speak ticked"); - std::string text_; + if (status() == BT::NodeStatus::IDLE || !is_goal_sent_) { + return on_idle(); + } + + return BT::NodeStatus::RUNNING; + /* std::string text_; getInput("say_text", text_); std::string param_; @@ -69,17 +80,83 @@ void Speak::on_tick() msg_dialog_action.data = 1; this->publisher_->publish(msg); - this->publisher_start_->publish(msg_dialog_action); + this->publisher_start_->publish(msg_dialog_action); */ } -BT::NodeStatus Speak::on_success() {return BT::NodeStatus::SUCCESS;} -} // namespace dialog -#include "behaviortree_cpp_v3/bt_factory.h" -BT_REGISTER_NODES(factory) +BT::NodeStatus Speak::on_idle() { - BT::NodeBuilder builder = [](const std::string & name, const BT::NodeConfiguration & config) { - return std::make_unique(name, "/say", config); - }; + auto goal = audio_common_msgs::action::TTS::Goal(); + + std::string text_; + getInput("say_text", text_); + + std::string param_; + getInput("param", param_); + + if (param_.length() > 0) { + goal.text = text_ + " " + param_; + } else { + goal.text = text_; + } + + auto msg = std_msgs::msg::String(); + auto msg_dialog_action = std_msgs::msg::Int8(); + + msg.data = goal.text; + msg_dialog_action.data = 1; + + this->publisher_->publish(msg); + this->publisher_start_->publish(msg_dialog_action); + + RCLCPP_INFO(node_->get_logger(), "Sending goal"); + + auto future_goal_handle = client_->async_send_goal(goal); + if (rclcpp::spin_until_future_complete( + node_->get_node_base_interface(), + future_goal_handle) != + rclcpp::FutureReturnCode::SUCCESS) + { + /* is_goal_sent_ = true; + auto result = *future_goal_handle.get(); + text_ = result.result->text; */ + RCLCPP_ERROR(node_->get_logger(), "send_goal failed"); + is_goal_sent_ = false; + return BT::NodeStatus::FAILURE; + } + + auto goal_handle = future_goal_handle.get(); + if (!goal_handle) { + RCLCPP_ERROR(node_->get_logger(), "Goal was rejected by server"); + return BT::NodeStatus::FAILURE; + } + + // Wait for the server to be done with the goal + auto result_future = client_->async_get_result(goal_handle); + + RCLCPP_INFO(node_->get_logger(), "Waiting for result"); + if (rclcpp::spin_until_future_complete(node_->get_node_base_interface(), result_future) != + rclcpp::FutureReturnCode::SUCCESS) + { + RCLCPP_ERROR(node_->get_logger(), "get result call failed :("); + return BT::NodeStatus::FAILURE; + } + + auto wrapped_result = result_future.get(); + + if(wrapped_result.code != rclcpp_action::ResultCode::SUCCEEDED) + { + RCLCPP_ERROR(node_->get_logger(), "Goal was rejected"); + return BT::NodeStatus::FAILURE; + } + + is_goal_sent_ = true; + + return BT::NodeStatus::SUCCESS; +} +} +// namespace dialog +#include "behaviortree_cpp_v3/bt_factory.h" +BT_REGISTER_NODES(factory) { - factory.registerBuilder("Speak", builder); + factory.registerNodeType("Speak"); }