add status dialog

igonzf · Apr 15, 2024 · 440452a · 440452a
1 parent 9f205c0
commit 440452a
Show file tree

Hide file tree

Showing 8 changed files with 139 additions and 131 deletions.
diff --git 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<whisper_msgs::action::STT>
-{
+    : public dialog::BtActionNode<whisper_msgs::action::STT> {
 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<std_msgs::msg::Int8>::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
@@ -16,34 +16,33 @@
 #define HRI__LISTEN_HPP_
 
 #include <algorithm>
+#include <cstdint>
 #include <string>
 
 #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<whisper_msgs::action::STT>
-{
+class Listen : public dialog::BtActionNode<whisper_msgs::action::STT> {
 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<std::string>("listen_text")});
   }
 
 private:
+  rclcpp::Publisher<std_msgs::msg::Int8>::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
@@ -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<llama_msgs::action::GenerateResponse>
-{
+    : public dialog::BtActionNode<llama_msgs::action::GenerateResponse> {
 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<std::string>("text"),
-        BT::InputPort<std::string>("intention"),
-        BT::OutputPort<std::string>("intention_value")});
+  static BT::PortsList providedPorts() {
+    return BT::PortsList({BT::InputPort<std::string>("text"),
+                          BT::InputPort<std::string>("intention"),
+                          BT::OutputPort<std::string>("intention_value")});
   }
 
 private:
   std::string intention_;
+  rclcpp::Publisher<std_msgs::msg::Int8>::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
@@ -16,38 +16,36 @@
 #define DIALOG__SPEAK_HPP_
 
 #include <algorithm>
+#include <cstdint>
 #include <string>
 
 #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<audio_common_msgs::action::TTS>
-{
+class Speak : public dialog::BtActionNode<audio_common_msgs::action::TTS> {
 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<std::string>("say_text"),
-        BT::InputPort<std::string>("param")});
+  static BT::PortsList providedPorts() {
+    return BT::PortsList({BT::InputPort<std::string>("say_text"),
+                          BT::InputPort<std::string>("param")});
   }
 
 private:
   rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_;
+  rclcpp::Publisher<std_msgs::msg::Int8>::SharedPtr publisher_start_;
   // rclcpp::Node::SharedPtr node_;
   //  rclcpp::ActionClient<audio_common_msgs::action::TTS>::SharedPtr
   //  tts_action_;

diff --git a/bt_nodes/hri/src/hri/dialog/DialogConfirmation.cpp b/bt_nodes/hri/src/hri/dialog/DialogConfirmation.cpp
@@ -16,46 +16,50 @@
 #include <utility>
 
 #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<whisper_msgs::action::STT>(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<whisper_msgs::action::STT>(xml_tag_name, action_name,
+                                                      conf) {
+  this->publisher_start_ =
+      node_->create_publisher<std_msgs::msg::Int8>("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<dialog::DialogConfirmation>(
-        name, "/whisper/listen",
-        config);
-    };
-
-  factory.registerBuilder<dialog::DialogConfirmation>(
-    "DialogConfirmation",
-    builder);
+  BT::NodeBuilder builder = [](const std::string &name,
+                               const BT::NodeConfiguration &config) {
+    return std::make_unique<dialog::DialogConfirmation>(name, "/whisper/listen",
+                                                        config);
+  };
+
+  factory.registerBuilder<dialog::DialogConfirmation>("DialogConfirmation",
+                                                      builder);
 }
diff --git 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 <cstdint>
 #include <string>
 #include <utility>
 
 #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<whisper_msgs::action::STT>(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<whisper_msgs::action::STT>(xml_tag_name, action_name,
+                                                      conf) {
+  this->publisher_start_ =
+      node_->create_publisher<std_msgs::msg::Int8>("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<dialog::Listen>(name, "whisper/listen", config);
-    };
+  BT::NodeBuilder builder = [](const std::string &name,
+                               const BT::NodeConfiguration &config) {
+    return std::make_unique<dialog::Listen>(name, "whisper/listen", config);
+  };
 
   factory.registerBuilder<dialog::Listen>("Listen", builder);
 }