Merge branch 'humble-devel' of https://github.com/CoreSenseEU/CoreSense4Home into humble-devel

Author: juandpenan

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

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>

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>

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;
+}

bt_nodes/configuration/src/configuration/init_receptionist.cpp

@@ -23,13 +23,13 @@ InitReceptionist::InitReceptionist(
 {
   config().blackboard->get("node", node_);
 
-  node_->declare_parameter("cam_frame", "head_front_camera_rgb_optical_frame");
+  node_->declare_parameter("cam_frame", "head_front_camera_color_optical_frame");
   node_->declare_parameter("manipulation_frame", "base_link");
   // node_->declare_parameter("party_wp",  std::vector<double>{0.0, 0.0, 0.0});
   // node_->declare_parameter("entrance_wp",  std::vector<double>{0.0, 0.0, 0.0});
   node_->declare_parameter("host_name", "John Doe");
   node_->declare_parameter("host_drink", "beer");
-  // node_->declare_parameter("waypoints_names", std::vector<std::string>{});
+  node_->declare_parameter("waypoints_names", std::vector<std::string>{});
 
   tf_buffer_ = std::make_shared<tf2_ros::Buffer>(node_->get_clock());
   tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
@@ -43,47 +43,58 @@ BT::NodeStatus InitReceptionist::tick()
 
   if (
     node_->has_parameter("cam_frame") && node_->has_parameter("manipulation_frame") &&
-    node_->has_parameter("host_name") && node_->has_parameter("host_drink"))
-  // && node_->has_parameter("waypoints_names"))
+    node_->has_parameter("host_name") && node_->has_parameter("host_drink")
+    && node_->has_parameter("waypoints_names"))
   // node_->has_parameter("party_wp") && node_->has_parameter("entrance_wp"))
   {
     node_->get_parameter("cam_frame", cam_frame_);
     node_->get_parameter("manipulation_frame", manipulation_frame_);
     node_->get_parameter("host_name", host_name_);
     node_->get_parameter("host_drink", host_drink_);
-    // node_->get_parameter("waypoints_names", wp_names_);
-
-    // for (auto wp : wp_names_) {
-    //   node_->declare_parameter("waypoints." + wp, std::vector<double>());
-    //   std::vector<double> wp_pos;
-    //   node_->get_parameter("waypoints." + wp, wp_pos);
-    //   geometry_msgs::msg::TransformStamped transform_msg;
-    //   tf2::Quaternion q;
-
-    //   if (wp.find("entrance")!=std::string::npos) {
-    //     setOutput("entrance_wp", wp);
-    //   } else if (wp.find("party")!=std::string::npos) {
-    //     setOutput("party_wp", wp);
-    //   }
-
-    //   q.setRPY(0, 0, wp_pos[2]);
-    //   transform_msg.header.frame_id = "map";
-
-    //   transform_msg.child_frame_id = wp;
-    //   transform_msg.transform.translation.x = wp_pos[0];
-    //   transform_msg.transform.translation.y = wp_pos[1];
-    //   transform_msg.transform.rotation = tf2::toMsg(q);
-    //   tf_static_broadcaster_->sendTransform(transform_msg);
-    //   rclcpp::spin_some(node_);
-    // }
+    node_->get_parameter("waypoints_names", wp_names_);
+
+    RCLCPP_INFO(
+      node_->get_logger(), "Waypoints to be initialized: [%s]",
+      std::to_string(wp_names_.size()).c_str());
+
+    for (auto wp : wp_names_) {
+      node_->declare_parameter("waypoints." + wp, std::vector<double>());
+      std::vector<double> wp_pos;
+      node_->get_parameter("waypoints." + wp, wp_pos);
+      RCLCPP_INFO(
+        node_->get_logger(), "Waypoint [%s] position: x: %.2f, y: %.2f, yaw: %.2f", wp.c_str(),
+        wp_pos[0], wp_pos[1], wp_pos[2]);
+      geometry_msgs::msg::TransformStamped transform_msg;
+      tf2::Quaternion q;
+
+      if (wp.find("entrance")!=std::string::npos) {
+        setOutput("entrance_wp", wp);
+        RCLCPP_INFO(
+          node_->get_logger(), "Entrance waypoint set to frame: [%s]", wp.c_str());
+      } else if (wp.find("party")!=std::string::npos) {
+        setOutput("party_wp", wp);
+      }
+
+      q.setRPY(0, 0, wp_pos[2]);
+      transform_msg.header.frame_id = "map";
+
+      transform_msg.child_frame_id = wp;
+      transform_msg.transform.translation.x = wp_pos[0];
+      transform_msg.transform.translation.y = wp_pos[1];
+      transform_msg.transform.rotation = tf2::toMsg(q);
+      tf_static_broadcaster_->sendTransform(transform_msg);
+      RCLCPP_INFO(
+        node_->get_logger(), "Published static transform for waypoint [%s]", wp.c_str());
+      rclcpp::spin_some(node_->get_node_base_interface());
+    }
 
     geometry_msgs::msg::TransformStamped transform_msg;
 
     transform_msg.header.frame_id = "base_link";
 
     transform_msg.child_frame_id = "attention_home";
     transform_msg.transform.translation.x = 1.5;
-    transform_msg.transform.translation.z = 1.5;
+    transform_msg.transform.translation.z = 0.5;
     tf_static_broadcaster_->sendTransform(transform_msg);
     rclcpp::spin_some(node_->get_node_base_interface());
 

bt_nodes/hri/CMakeLists.txt

@@ -21,6 +21,8 @@ find_package(std_srvs REQUIRED)
 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
@@ -38,6 +40,8 @@ set(dependencies
   gpsr_msgs
   std_srvs
   audio_common_msgs
+  perception_system_interfaces
+  kb_msgs
 )
 
 include_directories(include)
@@ -75,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)

bt_nodes/hri/include/hri/check_policy.hpp

@@ -26,6 +26,7 @@
 #include "rclcpp/rclcpp.hpp"
 #include "rclcpp_cascade_lifecycle/rclcpp_cascade_lifecycle.hpp"
 #include "std_msgs/msg/int8.hpp"
+#include "perception_system_interfaces/msg/detection_array.hpp"
 
 namespace dialog
 {
@@ -56,8 +57,8 @@ class CheckPolicy
   std::string image_topic_;
   bool value_;
   sensor_msgs::msg::Image::SharedPtr image_;
-  rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr image_sub_;
-  void image_callback(const sensor_msgs::msg::Image::SharedPtr msg);
+  rclcpp::Subscription<perception_system_interfaces::msg::DetectionArray>::SharedPtr image_sub_;
+  void image_callback(const perception_system_interfaces::msg::DetectionArray::SharedPtr msg);
 
 };
 

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_

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_

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>