add store and get kb info

igonzf · igonzf · commit 4812857d515f · 2025-12-16T13:11:06.000+01:00
diff --git a/bt_nodes/bt_test/CMakeLists.txt b/bt_nodes/bt_test/CMakeLists.txt
@@ -24,6 +24,7 @@ find_package(arm REQUIRED)
 find_package(tf2_ros REQUIRED)
 find_package(hri REQUIRED)
 find_package(perception_system_interfaces REQUIRED)
+find_package(kb_msgs REQUIRED)
 # find_package(backward_ros REQUIRED)
 
 set(CMAKE_CXX_STANDARD 17)
@@ -45,6 +46,7 @@ set(dependencies
   arm
   geometry_msgs
   perception_system_interfaces
+  kb_msgs
   # backward_ros
 )
 
@@ -204,6 +206,11 @@ add_executable(extract_person_description_test src/extract_person_description_te
 ament_target_dependencies(extract_person_description_test ${dependecies})
 target_link_libraries(extract_person_description_test perception::extract_person_description_bt_node perception::is_detected_bt_node hri::speak_bt_node)
 
+# store guest info test
+add_executable(store_guest_info_test src/store_guest_info_test.cpp)
+ament_target_dependencies(store_guest_info_test ${dependencies})
+target_link_libraries(store_guest_info_test hri::store_guest_info_bt_node hri::get_guest_info_bt_node)
+
 # gpsr test
 add_executable(gpsr_test src/gpsr_test.cpp)
 ament_target_dependencies(gpsr_test ${dependencies})
@@ -400,6 +407,7 @@ install(TARGETS
   extract_person_description_test
   music_test
   set_blackboard_test
+  store_guest_info_test
   
   gpsr_test
   gpsr_answerquiz_test
diff --git a/bt_nodes/bt_test/bt_xml/store_guest_info.xml b/bt_nodes/bt_test/bt_xml/store_guest_info.xml
@@ -0,0 +1,18 @@
+<?xml version="1.0"?>
+<root main_tree_to_execute="BehaviorTree">
+    <!-- ////////// -->
+    <BehaviorTree ID="BehaviorTree">
+        <Sequence>
+            <!-- <Action ID="StoreGuestInfo" guest_name="Irene" guest_drink="Cola" guest_description="Long hair and glasses."/> -->
+            <Action ID="GetGuestInfo" guest_name="{guest_name}" guest_drink="{guest_drink}" guest_description="{guest_description}"/>
+        </Sequence>
+    </BehaviorTree>
+    <!-- ////////// -->
+    <TreeNodesModel>
+        <Action ID="StoreGuestInfo">
+            <input_port name="guest_name"/>
+            <input_port name="guest_drink"/>
+            <input_port name="guest_description"/>
+        </Action>
+    </TreeNodesModel>
+</root>
diff --git a/bt_nodes/bt_test/package.xml b/bt_nodes/bt_test/package.xml
@@ -25,6 +25,7 @@
   <depend>tf2_ros</depend>
   <depend>hri</depend>
   <depend>perception_system_interfaces</depend>
+  <depend>kb_msgs</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
diff --git a/bt_nodes/bt_test/src/store_guest_info_test.cpp b/bt_nodes/bt_test/src/store_guest_info_test.cpp
@@ -0,0 +1,45 @@
+#include "ament_index_cpp/get_package_share_directory.hpp"
+#include "behaviortree_cpp_v3/behavior_tree.h"
+#include "behaviortree_cpp_v3/bt_factory.h"
+#include "behaviortree_cpp_v3/loggers/bt_zmq_publisher.h"
+#include "behaviortree_cpp_v3/utils/shared_library.h"
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp_cascade_lifecycle/rclcpp_cascade_lifecycle.hpp"
+
+int main(int argc, char * argv[])
+{
+  rclcpp::init(argc, argv);
+  auto node = std::make_shared<rclcpp_cascade_lifecycle::CascadeLifecycleNode>("store_guest_info_test_node");
+  BT::BehaviorTreeFactory factory;
+  BT::SharedLibrary loader;
+
+  factory.registerFromPlugin(loader.getOSName("store_guest_info_bt_node"));
+  factory.registerFromPlugin(loader.getOSName("get_guest_info_bt_node"));
+  std::string pkgpath = ament_index_cpp::get_package_share_directory("bt_test");
+  std::string xml_file = pkgpath + "/bt_xml/store_guest_info.xml";
+
+  auto blackboard = BT::Blackboard::create();
+  blackboard->set("node", node);
+  BT::Tree tree = factory.createTreeFromFile(xml_file, blackboard);
+
+  auto publisher_zmq = std::make_shared<BT::PublisherZMQ>(tree, 10, 2666, 2667);
+
+  node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE);
+  node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE);
+
+  rclcpp::Rate rate(10);
+
+  BT::NodeStatus status = BT::NodeStatus::RUNNING;
+  bool finish = false;
+  while (!finish && rclcpp::ok()) {
+    rclcpp::spin_some(node->get_node_base_interface());
+
+    status = tree.rootNode()->executeTick();
+    finish = (status == BT::NodeStatus::SUCCESS) || (status == BT::NodeStatus::FAILURE);
+
+    rate.sleep();
+  }
+  std::cout << "Store Guest Info Finished with status: " << status << std::endl;
+  rclcpp::shutdown();
+  return 0;
+}
diff --git a/bt_nodes/hri/CMakeLists.txt b/bt_nodes/hri/CMakeLists.txt
@@ -22,6 +22,7 @@ find_package(sensor_msgs REQUIRED)
 find_package(gpsr_msgs REQUIRED)
 find_package(audio_common_msgs REQUIRED)
 find_package(perception_system_interfaces REQUIRED)
+find_package(kb_msgs REQUIRED)
 set(CMAKE_CXX_STANDARD 17)
 
 set(dependencies
@@ -40,6 +41,7 @@ set(dependencies
   std_srvs
   audio_common_msgs
   perception_system_interfaces
+  kb_msgs
 )
 
 include_directories(include)
@@ -77,6 +79,12 @@ list(APPEND plugin_libs start_music_bt_node)
 add_library(stop_music_bt_node SHARED src/hri/stop_music.cpp)
 list(APPEND plugin_libs stop_music_bt_node)
 
+add_library(store_guest_info_bt_node SHARED src/hri/dialog/store_guest_info.cpp)
+list(APPEND plugin_libs store_guest_info_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)
+
 foreach(bt_plugin ${plugin_libs})
 ament_target_dependencies(${bt_plugin} ${dependencies})
 target_compile_definitions(${bt_plugin} PRIVATE BT_PLUGIN_EXPORT)
diff --git a/bt_nodes/hri/include/hri/dialog/get_guest_info.hpp b/bt_nodes/hri/include/hri/dialog/get_guest_info.hpp
@@ -0,0 +1,61 @@
+// 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__GEST_GUEST_INFO_HPP_
+#define HRI__GEST_GUEST_INFO_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 GetGuestInfo : public hri::BtServiceNode<
+    kb_msgs::srv::Query,
+    rclcpp_cascade_lifecycle::CascadeLifecycleNode>
+{
+public:
+  explicit GetGuestInfo(const std::string & xml_tag_name, const std::string & srv_name,
+    const BT::NodeConfiguration & conf);
+
+  void on_tick() override;
+  void on_result() override;
+
+  static BT::PortsList providedPorts()
+  {
+    return BT::PortsList(
+      {BT::OutputPort<std::string>("guest_name"),
+        BT::OutputPort<std::string>("guest_drink"),
+        BT::OutputPort<std::string>("guest_description"),
+      });
+  }
+
+private:
+  std::shared_ptr<rclcpp_cascade_lifecycle::CascadeLifecycleNode> node_;
+  rclcpp::Publisher<std_msgs::msg::String>::SharedPtr kb_publisher_;
+
+  std::string guest_name_, guest_drink_, guest_description_, guest_id_;
+};
+
+}  // namespace dialog
+
+#endif  // HRI__GEST_GUEST_INFO_HPP_
diff --git a/bt_nodes/hri/include/hri/dialog/store_guest_info.hpp b/bt_nodes/hri/include/hri/dialog/store_guest_info.hpp
@@ -0,0 +1,61 @@
+// 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__STORE_GUEST_INFO_HPP_
+#define HRI__STORE_GUEST_INFO_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 StoreGuestInfo : public hri::BtServiceNode<
+    kb_msgs::srv::Query,
+    rclcpp_cascade_lifecycle::CascadeLifecycleNode>
+{
+public:
+  explicit StoreGuestInfo(const std::string & xml_tag_name, const std::string & srv_name,
+    const BT::NodeConfiguration & conf);
+
+  void on_tick() override;
+  void on_result() override;
+
+  static BT::PortsList providedPorts()
+  {
+    return BT::PortsList(
+      {BT::InputPort<std::string>("guest_name"),
+        BT::InputPort<std::string>("guest_drink"),
+        BT::InputPort<std::string>("guest_description"),
+      });
+  }
+
+private:
+  std::shared_ptr<rclcpp_cascade_lifecycle::CascadeLifecycleNode> node_;
+  rclcpp::Publisher<std_msgs::msg::String>::SharedPtr kb_publisher_;
+
+  std::string guest_name_, guest_drink_, guest_description_;
+};
+
+}  // namespace dialog
+
+#endif  // HRI__STORE_GUEST_INFO_HPP_
diff --git a/bt_nodes/hri/package.xml b/bt_nodes/hri/package.xml
@@ -19,6 +19,8 @@
   <depend>shape_msgs</depend>
   <depend>std_msgs</depend>
   <depend>gpsr_msgs</depend>
+  <depend>perception_system_interfaces</depend>
+  <depend>kb_msgs</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
diff --git a/bt_nodes/hri/src/hri/dialog/get_guest_info.cpp b/bt_nodes/hri/src/hri/dialog/get_guest_info.cpp
@@ -0,0 +1,122 @@
+// 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/get_guest_info.hpp"
+#include <regex>
+
+using std::placeholders::_1;
+using namespace std::chrono_literals;
+
+
+namespace dialog
+{
+
+GetGuestInfo::GetGuestInfo(
+  const std::string & xml_tag_name, const std::string & srv_name,
+  const BT::NodeConfiguration & conf)
+: hri::BtServiceNode<
+    kb_msgs::srv::Query,
+    rclcpp_cascade_lifecycle::CascadeLifecycleNode>(xml_tag_name, srv_name, conf)
+{
+  config().blackboard->get("node", node_);
+  this->kb_publisher_= node_->create_publisher<std_msgs::msg::String>("/kb/remove_fact", 10);
+}
+
+void GetGuestInfo::on_tick()
+{
+  RCLCPP_DEBUG(node_->get_logger(), "[GetGuestInfo] ticked");
+  rclcpp::spin_some(node_->get_node_base_interface());
+  
+  // Patterns
+  request_->patterns.push_back("robot1 oro:attends ?guest");
+  request_->patterns.push_back("?guest oro:hasName ?name");
+  request_->patterns.push_back("?guest oro:hasFavoriteDrink ?drink");
+  request_->patterns.push_back("?guest oro:description ?desc");
+
+  // Vars
+  request_->vars.push_back("?guest");
+  request_->vars.push_back("?name");
+  request_->vars.push_back("?drink");
+  request_->vars.push_back("?desc");
+
+
+  setStatus(BT::NodeStatus::SUCCESS);
+}
+
+void GetGuestInfo::on_result()
+{
+  RCLCPP_DEBUG(node_->get_logger(), "[GetGuestInfo] result received");
+
+  if (!result_.error_msg.empty()) {
+    RCLCPP_ERROR(node_->get_logger(), "[GetGuestInfo] error");
+    setStatus(BT::NodeStatus::FAILURE);
+  }
+
+  std::regex guest_regex("\"guest\"\\s*:\\s*\"([^\"]*)\"");
+  std::regex name_regex("\"name\"\\s*:\\s*\"([^\"]*)\"");
+  std::regex drink_regex("\"drink\"\\s*:\\s*\"([^\"]*)\"");
+  std::regex desc_regex("\"desc\"\\s*:\\s*\"([^\"]*)\"");
+
+  std::smatch match;
+
+  if (std::regex_search(result_.json, match, guest_regex)) {
+    guest_id_ = match[1];
+  }
+
+  if (std::regex_search(result_.json, match, name_regex)) {
+    guest_name_ = match[1];
+  }
+
+  if (std::regex_search(result_.json, match, drink_regex)) {
+    guest_drink_ = match[1];
+  }
+
+  if (std::regex_search(result_.json, match, desc_regex)) {
+    guest_description_ = match[1];
+  }
+
+  setOutput("guest_name", guest_name_);
+  setOutput("guest_drink", guest_drink_);
+  setOutput("guest_description", guest_description_);
+
+  RCLCPP_INFO(
+    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
+  fact_msg.data = "robot1 oro:attends " + guest_id_;
+  kb_publisher_->publish(fact_msg);
+
+  setStatus(BT::NodeStatus::SUCCESS);
+}
+
+
+
+
+
+}  // namespace hri
+
+#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::GetGuestInfo>(
+        name, "/kb/query", config);
+    };
+
+  factory.registerBuilder<dialog::GetGuestInfo>("GetGuestInfo", builder);
+}
+
diff --git a/bt_nodes/hri/src/hri/dialog/store_guest_info.cpp b/bt_nodes/hri/src/hri/dialog/store_guest_info.cpp
