add extract_person_description bt_node

bt_nodes/bt_test/CMakeLists.txt

@@ -180,10 +180,16 @@ target_link_libraries(headposition_test
   motion::set_head_joint_position_bt_node
 )
 
+# filter object by type, size and weight test
 add_executable(filter_object_test src/filter_object_test.cpp)
 ament_target_dependencies(filter_object_test ${dependecies})
 target_link_libraries(filter_object_test perception::filter_object_bt_node)
 
+# extract the description of the person test
+add_executable(extract_person_description_test src/extract_person_description_test.cpp)
+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)
+
 # gpsr test
 add_executable(gpsr_test src/gpsr_test.cpp)
 ament_target_dependencies(gpsr_test ${dependencies})
@@ -371,6 +377,7 @@ install(TARGETS
   torsoheight_test
   headposition_test
   filter_object_test
+  extract_person_description_test
 
   gpsr_test
   gpsr_answerquiz_test

bt_nodes/bt_test/bt_xml/extract_person_description_test.xml

@@ -0,0 +1,42 @@
+<?xml version="1.0"?>
+<root main_tree_to_execute="BehaviorTree">
+    <!-- ////////// -->
+    <BehaviorTree ID="BehaviorTree">
+        <Sequence>
+            <RetryUntilSuccessful num_attempts="-1">
+                <Sequence>
+                    <Condition ID="IsDetected" confidence="0.4"
+                                               interest="person"
+                                               max_depth="6"
+                                               max_entities="1"
+                                               order="depth"
+                                               best_detection="{best_detection}"
+                                               cam_frame="base_footprint"/>
+                    <Action ID="ExtractPersonDescription" person_id="{best_detection}" interest="{interest}" description="{result}"/>
+                    <Action ID="Speak" say_text="The person is " param="{result}"/>
+                </Sequence>
+            </RetryUntilSuccessful>
+        </Sequence>
+    </BehaviorTree>
+    <!-- ////////// -->
+    <TreeNodesModel>
+        <Condition ID="IsDetected">
+            <input_port default="0.6" name="confidence"/>
+            <input_port default="person" name="interest"/>
+            <output_port name="frames">array of frames</output_port>
+            <input_port default="2" name="max_depth">value in meters</input_port>
+            <input_port default="1" name="max_entities"/>
+            <input_port default="depth_nearest" name="order">[depth_nearest, left_to_right]</input_port>
+        </Condition>
+        <Action ID="ExtractPersonDescription">
+            <input_port default="person_0" name="person_id"/>
+            <input_port name="interest"/>
+            <output_port name="description"/>
+        </Action>
+        <Action ID="Speak">
+            <input_port name="say_text"/>
+            <input_port name="param"/>
+        </Action>
+    </TreeNodesModel>
+    <!-- ////////// -->
+</root>

bt_nodes/bt_test/src/extract_person_description_test.cpp

@@ -0,0 +1,76 @@
+// 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 <memory>
+#include <string>
+
+#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);
+
+  rclcpp::NodeOptions options;
+  // options.automatically_declare_parameters_from_overrides(true);
+
+  auto node = std::make_shared<rclcpp_cascade_lifecycle::CascadeLifecycleNode>(
+      "extract_person_description_test", options);
+
+  BT::BehaviorTreeFactory factory;
+  BT::SharedLibrary loader;
+
+  factory.registerFromPlugin(loader.getOSName("is_detected_bt_node"));
+  factory.registerFromPlugin(loader.getOSName("speak_bt_node"));
+  factory.registerFromPlugin(
+      loader.getOSName("extract_person_description_bt_node"));
+
+  std::string pkgpath = ament_index_cpp::get_package_share_directory("bt_test");
+  std::string xml_file =
+      pkgpath + "/bt_xml/extract_person_description_test.xml";
+
+  auto blackboard = BT::Blackboard::create();
+
+  blackboard->set("node", node);
+  blackboard->set("interest", "pose");
+
+  // blackboard->set("gesture", "unknown");
+  BT::Tree tree = factory.createTreeFromFile(xml_file, blackboard);
+
+  auto publisher_zmq = std::make_shared<BT::PublisherZMQ>(tree, 10, 2666, 2667);
+  blackboard->set("publisher_zmq", publisher_zmq);
+
+  node->trigger_transition(
+      lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE);
+  node->trigger_transition(
+      lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE);
+
+  rclcpp::Rate rate(30);
+
+  bool finish = false;
+  while (!finish && rclcpp::ok()) {
+    rclcpp::spin_some(node->get_node_base_interface());
+
+    finish = tree.rootNode()->executeTick() != BT::NodeStatus::RUNNING;
+
+    rate.sleep();
+  }
+
+  rclcpp::shutdown();
+  return 0;
+}

bt_nodes/perception/CMakeLists.txt

@@ -101,6 +101,9 @@ list(APPEND plugin_libs filter_object_bt_node)
 add_library(filter_prev_detections_bt_node SHARED src/perception/filter_prev_detections.cpp)
 list(APPEND plugin_libs filter_prev_detections_bt_node)
 
+add_library(extract_person_description_bt_node SHARED src/perception/extract_person_description.cpp)
+list(APPEND plugin_libs extract_person_description_bt_node)
+
 foreach(bt_plugin ${plugin_libs})
 ament_target_dependencies(${bt_plugin} ${dependencies})
 target_compile_definitions(${bt_plugin} PRIVATE BT_PLUGIN_EXPORT)

bt_nodes/perception/include/perception/extract_person_description.hpp

@@ -0,0 +1,67 @@
+// 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.
+
+#ifndef PERCEPTION__EXTRACT_PERSON_DESCRIPTION_HPP_
+#define PERCEPTION__EXTRACT_PERSON_DESCRIPTION_HPP_
+
+#include <tf2/transform_datatypes.h>
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/static_transform_broadcaster.h>
+#include <tf2_ros/transform_listener.h>
+
+#include <algorithm>
+#include <string>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+
+#include "behaviortree_cpp_v3/behavior_tree.h"
+#include "behaviortree_cpp_v3/bt_factory.h"
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "geometry_msgs/msg/transform_stamped.hpp"
+#include "perception_system/PerceptionListener.hpp"
+#include "perception_system_interfaces/msg/detection_array.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "tf2_ros/transform_broadcaster.h"
+
+namespace perception {
+
+using pl = perception_system::PerceptionListener;
+
+class ExtractPersonDescription : public BT::ActionNodeBase {
+public:
+  explicit ExtractPersonDescription(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>("person_id"),
+                          BT::InputPort<std::string>("interest"),
+                          BT::OutputPort<std::string>("description")});
+  }
+
+private:
+  std::shared_ptr<rclcpp_cascade_lifecycle::CascadeLifecycleNode> node_;
+
+  std::string person_id_;
+  std::string interest_;
+  std::string description_;
+
+  std::map<std::string, std::vector<int>> gestures_;
+  std::map<int, std::string> pose_names_;
+};
+
+} // namespace perception
+
+#endif // PERCEPTION__EXTRACT_PERSON_DESCRIPTION_HPP_

bt_nodes/perception/src/perception/extract_person_description.cpp

@@ -0,0 +1,116 @@
+// 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 "perception/extract_person_description.hpp"
+
+#include <limits>
+#include <string>
+#include <utility>
+
+#include "behaviortree_cpp_v3/behavior_tree.h"
+#include "perception_system/PerceptionUtils.hpp"
+
+namespace perception {
+
+using namespace std::chrono_literals;
+using namespace std::placeholders;
+
+ExtractPersonDescription::ExtractPersonDescription(
+    const std::string &xml_tag_name, const BT::NodeConfiguration &conf)
+    : BT::ActionNodeBase(xml_tag_name, conf), gestures_({
+                                                  {"pointing_right", {0, 1}},
+                                                  {"pointing_left", {3, 4}},
+
+                                                  {"waving", {5, 6, 7}},
+                                                  {"rising_left", {6}},
+                                                  {"rising_left", {6}},
+                                              }),
+      pose_names_({
+          {0, "lying"},
+          {1, "sitting"},
+          {2, "standing"},
+          {-1, "unknown"},
+      }) {
+  config().blackboard->get("node", node_);
+
+  // pl::getInstance(node_)->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE);
+  // pl::getInstance(node_)->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE);
+}
+
+BT::NodeStatus ExtractPersonDescription::tick() {
+
+  getInput("person_id", person_id_);
+
+  pl::getInstance(node_)->set_interest(person_id_, true);
+  pl::getInstance(node_)->update(30);
+
+  RCLCPP_INFO(node_->get_logger(), "[ExtractPersonDescription] Describe %s",
+              person_id_.c_str());
+
+  auto detections = pl::getInstance(node_)->get_by_id(person_id_);
+
+  if (detections.empty()) {
+    RCLCPP_ERROR(node_->get_logger(),
+                 "[ExtractPersonDescription] No detections");
+    return BT::NodeStatus::FAILURE;
+  }
+
+  RCLCPP_DEBUG(node_->get_logger(),
+               "[ExtractPersonDescription] Processing detections...");
+
+  getInput("interest", interest_);
+
+  if (interest_ == "pose") {
+
+    description_ = pose_names_[detections[0].body_pose];
+    RCLCPP_INFO(node_->get_logger(),
+                "[ExtractPersonDescription] The pose of the person is: %s",
+                description_.c_str());
+    setOutput("description", description_);
+
+    return BT::NodeStatus::SUCCESS;
+  }
+
+  if (interest_ == "gesture") {
+    int pointing_direction = detections[0].pointing_direction;
+
+    for (const auto &gesture : gestures_) {
+      const auto &key = gesture.first;
+      const auto &direction = gesture.second;
+
+      if (std::find(direction.begin(), direction.end(), pointing_direction) !=
+          direction.end()) {
+        description_ = key;
+        setOutput("description", description_);
+        RCLCPP_INFO(
+            node_->get_logger(),
+            "[ExtractPersonDescription] The gesture of the person is: %s",
+            description_.c_str());
+        return BT::NodeStatus::SUCCESS;
+      }
+    }
+  }
+
+  return BT::NodeStatus::SUCCESS;
+}
+void ExtractPersonDescription::halt() {
+  RCLCPP_DEBUG(node_->get_logger(), "ExtractPersonDescription halted");
+}
+
+} // namespace perception
+
+BT_REGISTER_NODES(factory) {
+  factory.registerNodeType<perception::ExtractPersonDescription>(
+      "ExtractPersonDescription");
+}