add remove_guest

Author: igonzf

bt_nodes/hri/CMakeLists.txt

@@ -88,6 +88,9 @@ list(APPEND plugin_libs get_attended_guest_bt_node)
 add_library(get_guest_info_bt_node SHARED src/hri/dialog/get_guest_info.cpp)
 list(APPEND plugin_libs get_guest_info_bt_node)
 
+add_library(remove_guest_attended_bt_node SHARED src/hri/dialog/remove_guest_attended.cpp)
+list(APPEND plugin_libs remove_guest_attended_bt_node)
+
 foreach(bt_plugin ${plugin_libs})
 ament_target_dependencies(${bt_plugin} ${dependencies})
 target_compile_definitions(${bt_plugin} PRIVATE BT_PLUGIN_EXPORT)

bt_nodes/hri/include/hri/dialog/remove_guest_attended.hpp

@@ -0,0 +1,57 @@
+// Copyright 2024 Intelligent Robotics Lab - Gentlebots
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions andGO2OBJECT
+// limitations under the License.
+
+#ifndef HRI__REMOVE_GUEST_ATTENDED_HPP_
+#define HRI__REMOVE_GUEST_ATTENDED_HPP_
+
+#include <chrono>
+#include <functional>
+
+#include "behaviortree_cpp_v3/behavior_tree.h"
+#include "behaviortree_cpp_v3/bt_factory.h"
+#include "std_msgs/msg/string.hpp"
+#include "kb_msgs/srv/query.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp_cascade_lifecycle/rclcpp_cascade_lifecycle.hpp"
+#include "hri/bt_service_node.hpp"
+
+namespace dialog
+{
+
+class RemoveGuestAttended : public BT::ActionNodeBase
+{
+public:
+  explicit RemoveGuestAttended(const std::string & xml_tag_name, const BT::NodeConfiguration & conf);
+
+  void halt();
+  BT::NodeStatus tick();
+
+  static BT::PortsList providedPorts()
+  {
+    return BT::PortsList(
+      {
+        BT::InputPort<std::string>("guest_attended")
+      }
+    );
+  }
+
+private:
+  std::shared_ptr<rclcpp_cascade_lifecycle::CascadeLifecycleNode> node_;
+  rclcpp::Publisher<std_msgs::msg::String>::SharedPtr kb_publisher_;
+  std::string guest_attending_id_;
+};
+
+}  // namespace dialog
+
+#endif  // HRI__REMOVE_GUEST_ATTENDED_HPP_

bt_nodes/hri/src/hri/dialog/get_attended_guest.cpp

@@ -63,14 +63,18 @@ void GetAttendedGuest::on_result()
     guest_id_ = match[1];
   }
 
-  setOutput("guest_attended", guest_id_);
+  if (guest_id_.empty()) {
+    RCLCPP_ERROR(node_->get_logger(), "[GetAttendedGuest] No attended guest found in the result");
+    setStatus(BT::NodeStatus::FAILURE);
+  }else{
 
-  RCLCPP_INFO(
-    node_->get_logger(), "[GetAttendedGuest] Guest attended: %s", guest_id_.c_str());
+    setOutput("guest_attended", guest_id_);
 
-  std_msgs::msg::String fact_msg;
+    RCLCPP_INFO(
+    node_->get_logger(), "[GetAttendedGuest] Guest attended: %s", guest_id_.c_str());
 
-  setStatus(BT::NodeStatus::SUCCESS);
+    setStatus(BT::NodeStatus::SUCCESS);
+  }
 }
 
 

bt_nodes/hri/src/hri/dialog/get_guest_info.cpp

@@ -109,14 +109,6 @@ void GetGuestInfo::on_result()
     node_->get_logger(), "[GetGuestInfo] Guest info retrieved: Name: %s, Drink: %s, Description: %s",
     guest_name_.c_str(), guest_drink_.c_str(), guest_description_.c_str());
 
-  std_msgs::msg::String fact_msg;
-
-  // Delete attending fact
-  if (!guest_attending_id_.empty()) {
-    fact_msg.data = "robot1 oro:attends " + guest_attending_id_;
-    kb_publisher_->publish(fact_msg);
-    RCLCPP_INFO(node_->get_logger(), "[GetGuestInfo] Removing fact: robot1 oro:attends %s", guest_id_.c_str());
-  }
 
   setStatus(BT::NodeStatus::SUCCESS);
 }

bt_nodes/hri/src/hri/dialog/remove_guest_attended.cpp

@@ -0,0 +1,63 @@
+// Copyright 2024 Intelligent Robotics Lab - Gentlebots
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "hri/dialog/remove_guest_attended.hpp"
+#include <regex>
+
+using std::placeholders::_1;
+using namespace std::chrono_literals;
+
+
+namespace dialog
+{
+
+RemoveGuestAttended::RemoveGuestAttended(
+  const std::string & xml_tag_name, const BT::NodeConfiguration & conf)
+: BT::ActionNodeBase(xml_tag_name, conf)
+{
+  config().blackboard->get("node", node_);
+  this->kb_publisher_= node_->create_publisher<std_msgs::msg::String>("/kb/remove_fact", 10);
+}
+
+BT::NodeStatus RemoveGuestAttended::tick()
+{
+  RCLCPP_DEBUG(node_->get_logger(), "[RemoveGuestAttended] ticked");
+
+  getInput("guest_attended", guest_attending_id_);
+
+  std_msgs::msg::String fact_msg;
+
+  if (!guest_attending_id_.empty()) {
+    fact_msg.data = "robot1 oro:attends " + guest_attending_id_;
+    kb_publisher_->publish(fact_msg);
+    RCLCPP_INFO(node_->get_logger(), "[RemoveGuestAttended] Removing fact: robot1 oro:attends %s", guest_attending_id_.c_str());
+  }else{
+    return BT::NodeStatus::FAILURE;
+  }
+
+  return BT::NodeStatus::SUCCESS;
+
+}
+
+void RemoveGuestAttended::halt() {}
+
+
+}  // namespace hri
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+BT_REGISTER_NODES(factory)
+{
+
+  factory.registerNodeType<dialog::RemoveGuestAttended>("RemoveGuestAttended");
+}

bt_nodes/hri/src/hri/dialog/store_guest_info.cpp

@@ -75,6 +75,12 @@ std::string obtain_guest_id(const std::string & json)
   return "guest" + std::to_string(max_id + 1);
 }
 
+static std::string unescape_latex_like(std::string s)
+{
+  static const std::regex rx(R"(\\([,.;:!?()\[\]{}\-_%&#$]))");
+  return std::regex_replace(s, rx, "$1");
+}
+
 static std::string escape_turtle_literal(std::string s)
 {
   // 1) Escape backslashes
@@ -92,6 +98,10 @@ static std::string escape_turtle_literal(std::string s)
     s.replace(pos, 1, "\\n");
   }
 
+  for (size_t pos = 0; (pos = s.find('\r', pos)) != std::string::npos; pos += 2) {
+    s.replace(pos, 1, "\\r");
+  }
+
   return s;
 }
 
@@ -121,6 +131,11 @@ void StoreGuestInfo::on_result()
   kb_publisher_->publish(fact_msg);
 
   if (!guest_description_.empty()) {
+    if (guest_description_.find('\\') != std::string::npos) {
+      RCLCPP_WARN(node_->get_logger(), "guest_description (pre-fix): '%s'",
+                  guest_description_.c_str());
+    }
+    guest_description_ = unescape_latex_like(guest_description_);
     guest_description_ = escape_turtle_literal(guest_description_);
     fact_msg.data = guest_id + " oro:description \"" + guest_description_ + "\"";
     kb_publisher_->publish(fact_msg);

robocup_bringup/launch/dialog.launch.py

@@ -130,7 +130,7 @@ def generate_launch_description():
     ld.add_action(llava_cmd)
     ld.add_action(audio_common_tts_node)
     ld.add_action(audio_common_player_node)
-    ld.add_action(kb_cmd)
+    # ld.add_action(kb_cmd)
 
     #ld.add_action(music_player_node)