change Listen from BtActionNode to ActionNodeBase

bt_nodes/hri/include/hri/dialog/Listen.hpp

@@ -24,6 +24,7 @@
 #include "hri/dialog/BTActionNode.hpp"
 
 #include "rclcpp/rclcpp.hpp"
+#include "rclcpp_action/rclcpp_action.hpp"
 #include "rclcpp_cascade_lifecycle/rclcpp_cascade_lifecycle.hpp"
 
 #include "std_msgs/msg/int8.hpp"
@@ -32,24 +33,28 @@
 namespace dialog
 {
 
-class Listen : public dialog::BtActionNode<
-    whisper_msgs::action::STT, rclcpp_cascade_lifecycle::CascadeLifecycleNode>
+class Listen : public BT::ActionNodeBase
 {
 public:
   explicit Listen(
-    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()
   {
     return BT::PortsList({BT::OutputPort<std::string>("listen_text")});
   }
 
 private:
-  rclcpp::Publisher<std_msgs::msg::Int8>::SharedPtr publisher_start_;
+  BT::NodeStatus on_idle();
+  std::shared_ptr<rclcpp_cascade_lifecycle::CascadeLifecycleNode> node_;
+  rclcpp_lifecycle::LifecyclePublisher<std_msgs::msg::Int8>::SharedPtr publisher_start_;
+  std::shared_ptr<rclcpp_action::Client<whisper_msgs::action::STT>> client_;
+  bool is_goal_sent_ = false;
+  std::string text_ = "";
 };
 
 }  // namespace dialog

bt_nodes/hri/src/hri/dialog/Listen.cpp

@@ -30,45 +30,103 @@ using namespace std::chrono_literals;
 using namespace std::placeholders;
 
 Listen::Listen(
-  const std::string & xml_tag_name, const std::string & action_name,
+  const std::string & xml_tag_name,
   const BT::NodeConfiguration & conf)
-: dialog::BtActionNode<whisper_msgs::action::STT, rclcpp_cascade_lifecycle::CascadeLifecycleNode>(
-    xml_tag_name, action_name, conf)
+: BT::ActionNodeBase(
+    xml_tag_name, conf)
 {
+  config().blackboard->get("node", node_);
   publisher_start_ = node_->create_publisher<std_msgs::msg::Int8>("dialog_action", 10);
+  publisher_start_->on_activate();
+
+  client_ = rclcpp_action::create_client<whisper_msgs::action::STT>(
+    node_, "whisper/listen");
+
+}
+
+void Listen::halt()
+{
+  RCLCPP_INFO(node_->get_logger(), "Listen halted");
 }
 
-void Listen::on_tick()
+BT::NodeStatus Listen::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;
 
   publisher_start_->publish(msg_dialog_action);
-}
 
-BT::NodeStatus Listen::on_success()
-{
-  fprintf(stderr, "%s\n", result_.result->text.c_str());
+  if (status() == BT::NodeStatus::IDLE || !is_goal_sent_) {
+    return on_idle();
+  }
 
-  if (result_.result->text.size() == 0) {
+  if (text_.size() == 0) {
+    RCLCPP_ERROR(node_->get_logger(), "whisper not listen anything");
     return BT::NodeStatus::FAILURE;
   }
 
-  setOutput("listen_text", result_.result->text);
+  fprintf(stderr, "%s\n", text_.c_str());
+
+  setOutput("listen_text", text_);
   return BT::NodeStatus::SUCCESS;
 }
 
-}  // namespace dialog
-#include "behaviortree_cpp_v3/bt_factory.h"
-BT_REGISTER_NODES(factory)
+BT::NodeStatus Listen::on_idle()
 {
-  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);
+  auto goal = whisper_msgs::action::STT::Goal();
+
+  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)
+  {
+
+    RCLCPP_ERROR(node_->get_logger(), "send_goal failed");
+    is_goal_sent_ = false;
+    return BT::NodeStatus::RUNNING;
+  }
+
+  auto goal_handle = future_goal_handle.get();
+  if (!goal_handle) {
+    RCLCPP_ERROR(node_->get_logger(), "Goal was rejected by server");
+    return BT::NodeStatus::RUNNING;
+  }
+
+  // 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::RUNNING;
+  }
+
+  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::RUNNING;
+  }
+
+  is_goal_sent_ = true;
+  text_ = wrapped_result.result->text; 
+
+  return BT::NodeStatus::RUNNING;
 }
+
+}  // namespace dialog
+#include "behaviortree_cpp_v3/bt_factory.h"
+BT_REGISTER_NODES(factory) {
+
+  factory.registerNodeType<dialog::Listen>("Listen");
+}