diff --git a/bt_nodes/hri/include/hri/dialog/DialogConfirmation.hpp b/bt_nodes/hri/include/hri/dialog/DialogConfirmation.hpp index 8a04a9f..fe14860 100644 --- a/bt_nodes/hri/include/hri/dialog/DialogConfirmation.hpp +++ b/bt_nodes/hri/include/hri/dialog/DialogConfirmation.hpp @@ -22,26 +22,25 @@ #include "behaviortree_cpp_v3/bt_factory.h" #include "hri/dialog/BTActionNode.hpp" #include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/int8.hpp" #include "whisper_msgs/action/stt.hpp" -namespace dialog -{ +namespace dialog { class DialogConfirmation - : public dialog::BtActionNode -{ + : public dialog::BtActionNode { public: - explicit DialogConfirmation( - const std::string & xml_tag_name, - const std::string & action_name, - const BT::NodeConfiguration & conf); + explicit DialogConfirmation(const std::string &xml_tag_name, + const std::string &action_name, + const BT::NodeConfiguration &conf); void on_tick() override; BT::NodeStatus on_success() override; - static BT::PortsList providedPorts() {return BT::PortsList({});} + static BT::PortsList providedPorts() { return BT::PortsList({}); } private: + rclcpp::Publisher::SharedPtr publisher_start_; }; } // namespace dialog diff --git a/bt_nodes/hri/include/hri/dialog/Listen.hpp b/bt_nodes/hri/include/hri/dialog/Listen.hpp index ebc7be5..83d9589 100644 --- a/bt_nodes/hri/include/hri/dialog/Listen.hpp +++ b/bt_nodes/hri/include/hri/dialog/Listen.hpp @@ -16,34 +16,33 @@ #define HRI__LISTEN_HPP_ #include +#include #include #include "behaviortree_cpp_v3/behavior_tree.h" #include "behaviortree_cpp_v3/bt_factory.h" #include "hri/dialog/BTActionNode.hpp" #include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/int8.hpp" #include "whisper_msgs/action/stt.hpp" -namespace dialog -{ +namespace dialog { -class Listen : public dialog::BtActionNode -{ +class Listen : public dialog::BtActionNode { public: - explicit Listen( - const std::string & xml_tag_name, - const std::string & action_name, - const BT::NodeConfiguration & conf); + explicit Listen(const std::string &xml_tag_name, + const std::string &action_name, + const BT::NodeConfiguration &conf); void on_tick() override; BT::NodeStatus on_success() override; - static BT::PortsList providedPorts() - { + static BT::PortsList providedPorts() { return BT::PortsList({BT::OutputPort("listen_text")}); } private: + rclcpp::Publisher::SharedPtr publisher_start_; }; } // namespace dialog diff --git a/bt_nodes/hri/include/hri/dialog/Query.hpp b/bt_nodes/hri/include/hri/dialog/Query.hpp index b68b532..55d6b3d 100644 --- a/bt_nodes/hri/include/hri/dialog/Query.hpp +++ b/bt_nodes/hri/include/hri/dialog/Query.hpp @@ -23,32 +23,29 @@ #include "hri/dialog/BTActionNode.hpp" #include "llama_msgs/action/generate_response.hpp" #include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/int8.hpp" -namespace dialog -{ +namespace dialog { class Query - : public dialog::BtActionNode -{ + : public dialog::BtActionNode { public: - explicit Query( - const std::string & xml_tag_name, - const std::string & action_name, - const BT::NodeConfiguration & conf); + explicit Query(const std::string &xml_tag_name, + const std::string &action_name, + const BT::NodeConfiguration &conf); void on_tick() override; BT::NodeStatus on_success() override; - static BT::PortsList providedPorts() - { - return BT::PortsList( - {BT::InputPort("text"), - BT::InputPort("intention"), - BT::OutputPort("intention_value")}); + static BT::PortsList providedPorts() { + return BT::PortsList({BT::InputPort("text"), + BT::InputPort("intention"), + BT::OutputPort("intention_value")}); } private: std::string intention_; + rclcpp::Publisher::SharedPtr publisher_start_; }; } // namespace dialog diff --git a/bt_nodes/hri/include/hri/dialog/Speak.hpp b/bt_nodes/hri/include/hri/dialog/Speak.hpp index 55a9647..0a98749 100644 --- a/bt_nodes/hri/include/hri/dialog/Speak.hpp +++ b/bt_nodes/hri/include/hri/dialog/Speak.hpp @@ -16,38 +16,36 @@ #define DIALOG__SPEAK_HPP_ #include +#include #include #include "audio_common_msgs/action/tts.hpp" #include "behaviortree_cpp_v3/behavior_tree.h" #include "behaviortree_cpp_v3/bt_factory.h" #include "hri/dialog/BTActionNode.hpp" -#include "std_msgs/msg/string.hpp" #include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/int8.hpp" +#include "std_msgs/msg/string.hpp" -namespace dialog -{ +namespace dialog { -class Speak : public dialog::BtActionNode -{ +class Speak : public dialog::BtActionNode { public: - explicit Speak( - const std::string & xml_tag_name, - const std::string & action_name, - const BT::NodeConfiguration & conf); + explicit Speak(const std::string &xml_tag_name, + const std::string &action_name, + const BT::NodeConfiguration &conf); void on_tick() override; BT::NodeStatus on_success() override; - static BT::PortsList providedPorts() - { - return BT::PortsList( - {BT::InputPort("say_text"), - BT::InputPort("param")}); + static BT::PortsList providedPorts() { + return BT::PortsList({BT::InputPort("say_text"), + BT::InputPort("param")}); } private: rclcpp::Publisher::SharedPtr publisher_; + rclcpp::Publisher::SharedPtr publisher_start_; // 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 5246ca7..02cf680 100644 --- a/bt_nodes/hri/src/hri/dialog/DialogConfirmation.cpp +++ b/bt_nodes/hri/src/hri/dialog/DialogConfirmation.cpp @@ -16,46 +16,50 @@ #include #include "hri/dialog/DialogConfirmation.hpp" +#include "std_msgs/msg/int8.hpp" #include "whisper_msgs/action/stt.hpp" #include "behaviortree_cpp_v3/behavior_tree.h" -namespace dialog -{ +namespace dialog { using namespace std::chrono_literals; using namespace std::placeholders; -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) {} +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) { + this->publisher_start_ = + node_->create_publisher("dialog_action", 10); +} -void DialogConfirmation::on_tick() -{ +void DialogConfirmation::on_tick() { RCLCPP_DEBUG(node_->get_logger(), "DialogConfirmation ticked"); std::string text_; // getInput("prompt", text_); goal_ = whisper_msgs::action::STT::Goal(); + auto msg_dialog_action = std_msgs::msg::Int8(); + + msg_dialog_action.data = 0; + + this->publisher_start_->publish(msg_dialog_action); // goal_.text = text_; } -BT::NodeStatus DialogConfirmation::on_success() -{ +BT::NodeStatus DialogConfirmation::on_success() { fprintf(stderr, "%s\n", result_.result->text.c_str()); if (result_.result->text.size() == 0) { return BT::NodeStatus::FAILURE; } - std::transform( - result_.result->text.begin(), result_.result->text.end(), - result_.result->text.begin(), - [](unsigned char c) {return std::tolower(c);}); + std::transform(result_.result->text.begin(), result_.result->text.end(), + result_.result->text.begin(), + [](unsigned char c) { return std::tolower(c); }); if (result_.result->text.find("yes") != std::string::npos) { return BT::NodeStatus::SUCCESS; } else { @@ -66,14 +70,12 @@ BT::NodeStatus DialogConfirmation::on_success() } // namespace dialog #include "behaviortree_cpp_v3/bt_factory.h" BT_REGISTER_NODES(factory) { - BT::NodeBuilder builder = [](const std::string & name, - const BT::NodeConfiguration & config) { - return std::make_unique( - name, "/whisper/listen", - config); - }; - - factory.registerBuilder( - "DialogConfirmation", - builder); + BT::NodeBuilder builder = [](const std::string &name, + const BT::NodeConfiguration &config) { + return std::make_unique(name, "/whisper/listen", + config); + }; + + factory.registerBuilder("DialogConfirmation", + builder); } diff --git a/bt_nodes/hri/src/hri/dialog/Listen.cpp b/bt_nodes/hri/src/hri/dialog/Listen.cpp index 694993b..539de22 100644 --- a/bt_nodes/hri/src/hri/dialog/Listen.cpp +++ b/bt_nodes/hri/src/hri/dialog/Listen.cpp @@ -12,36 +12,42 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include #include #include #include "hri/dialog/Listen.hpp" +#include "std_msgs/msg/int8.hpp" #include "whisper_msgs/action/stt.hpp" #include "behaviortree_cpp_v3/behavior_tree.h" -namespace dialog -{ +namespace dialog { using namespace std::chrono_literals; 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) {} +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) { + this->publisher_start_ = + node_->create_publisher("dialog_action", 10); +} -void Listen::on_tick() -{ +void Listen::on_tick() { RCLCPP_DEBUG(node_->get_logger(), "Listen ticked"); std::string text_; goal_ = whisper_msgs::action::STT::Goal(); + auto msg_dialog_action = std_msgs::msg::Int8(); + + msg_dialog_action.data = 0; + + this->publisher_start_->publish(msg_dialog_action); } -BT::NodeStatus Listen::on_success() -{ +BT::NodeStatus Listen::on_success() { fprintf(stderr, "%s\n", result_.result->text.c_str()); if (result_.result->text.size() == 0) { @@ -55,10 +61,10 @@ BT::NodeStatus Listen::on_success() } // namespace dialog #include "behaviortree_cpp_v3/bt_factory.h" BT_REGISTER_NODES(factory) { - BT::NodeBuilder builder = [](const std::string & name, - const BT::NodeConfiguration & config) { - return std::make_unique(name, "whisper/listen", config); - }; + BT::NodeBuilder builder = [](const std::string &name, + const BT::NodeConfiguration &config) { + return std::make_unique(name, "whisper/listen", config); + }; factory.registerBuilder("Listen", builder); } diff --git a/bt_nodes/hri/src/hri/dialog/Query.cpp b/bt_nodes/hri/src/hri/dialog/Query.cpp index e83effb..9192396 100644 --- a/bt_nodes/hri/src/hri/dialog/Query.cpp +++ b/bt_nodes/hri/src/hri/dialog/Query.cpp @@ -19,39 +19,40 @@ #include "hri/dialog/Query.hpp" #include "llama_msgs/action/generate_response.hpp" +#include "std_msgs/msg/int8.hpp" #include "behaviortree_cpp_v3/behavior_tree.h" -namespace dialog -{ +namespace dialog { using namespace std::chrono_literals; using namespace std::placeholders; using json = nlohmann::json; -Query::Query( - const std::string & xml_tag_name, const std::string & action_name, - const BT::NodeConfiguration & conf) -: dialog::BtActionNode( - xml_tag_name, action_name, conf) {} +Query::Query(const std::string &xml_tag_name, const std::string &action_name, + const BT::NodeConfiguration &conf) + : dialog::BtActionNode( + xml_tag_name, action_name, conf) { + this->publisher_start_ = + node_->create_publisher("dialog_action", 10); +} -void Query::on_tick() -{ +void Query::on_tick() { RCLCPP_DEBUG(node_->get_logger(), "Query ticked"); std::string text_; getInput("text", text_); getInput("intention", intention_); std::string prompt_ = - "Given the sentence \"" + text_ + "\", extract the " + intention_ + - " from the sentence and return " - "it with the following JSON format:\n" + - "{\n\t\"intention\": \"word extracted in the sentence\"\n}"; + "Given the sentence \"" + text_ + "\", extract the " + intention_ + + " from the sentence and return " + "it with the following JSON format:\n" + + "{\n\t\"intention\": \"word extracted in the sentence\"\n}"; goal_.prompt = prompt_; goal_.reset = true; goal_.sampling_config.temp = 0.0; goal_.sampling_config.grammar = - R"(root ::= object + R"(root ::= object value ::= object | array | string | number | ("true" | "false" | "null") ws object ::= @@ -76,15 +77,19 @@ number ::= ("-"? ([0-9] | [1-9] [0-9]*)) ("." [0-9]+)? ([eE] [-+]? [0-9]+)? ws # Optional space: by convention, applied in this grammar after literal chars when allowed ws ::= ([ \t\n] ws)?)"; + + auto msg_dialog_action = std_msgs::msg::Int8(); + + msg_dialog_action.data = 2; + + this->publisher_start_->publish(msg_dialog_action); } -BT::NodeStatus Query::on_success() -{ +BT::NodeStatus Query::on_success() { fprintf(stderr, "%s\n", result_.result->response.text.c_str()); if (result_.result->response.text.empty() || - result_.result->response.text == "{}") - { + result_.result->response.text == "{}") { return BT::NodeStatus::FAILURE; } @@ -105,12 +110,11 @@ BT::NodeStatus Query::on_success() } // namespace dialog #include "behaviortree_cpp_v3/bt_factory.h" BT_REGISTER_NODES(factory) { - BT::NodeBuilder builder = [](const std::string & name, - const BT::NodeConfiguration & config) { - return std::make_unique( - name, "/llama/generate_response", - config); - }; + BT::NodeBuilder builder = [](const std::string &name, + const BT::NodeConfiguration &config) { + return std::make_unique(name, "/llama/generate_response", + config); + }; factory.registerBuilder("Query", builder); } diff --git a/bt_nodes/hri/src/hri/dialog/Speak.cpp b/bt_nodes/hri/src/hri/dialog/Speak.cpp index 204b8a6..f374815 100644 --- a/bt_nodes/hri/src/hri/dialog/Speak.cpp +++ b/bt_nodes/hri/src/hri/dialog/Speak.cpp @@ -12,33 +12,33 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include #include #include #include "audio_common_msgs/action/tts.hpp" #include "hri/dialog/Speak.hpp" +#include "std_msgs/msg/int8.hpp" #include "std_msgs/msg/string.hpp" #include "behaviortree_cpp_v3/behavior_tree.h" -namespace dialog -{ +namespace dialog { using namespace std::chrono_literals; 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) -{ +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) { node_ = config().blackboard->get("node"); - this->publisher_ = node_->create_publisher("say_text", 10); + this->publisher_ = + node_->create_publisher("say_text", 10); + this->publisher_start_ = + node_->create_publisher("dialog_action", 10); } -void -Speak::on_tick() -{ +void Speak::on_tick() { RCLCPP_DEBUG(node_->get_logger(), "Speak ticked"); std::string text_; @@ -54,20 +54,23 @@ Speak::on_tick() } 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); } -BT::NodeStatus Speak::on_success() {return BT::NodeStatus::SUCCESS;} +BT::NodeStatus Speak::on_success() { return BT::NodeStatus::SUCCESS; } } // namespace dialog #include "behaviortree_cpp_v3/bt_factory.h" -BT_REGISTER_NODES(factory) -{ - BT::NodeBuilder builder = [](const std::string & name, - const BT::NodeConfiguration & config) { - return std::make_unique(name, "/say", config); - }; +BT_REGISTER_NODES(factory) { + BT::NodeBuilder builder = [](const std::string &name, + const BT::NodeConfiguration &config) { + return std::make_unique(name, "/say", config); + }; factory.registerBuilder("Speak", builder); }