diff --git a/bt_nodes/bt_test/CMakeLists.txt b/bt_nodes/bt_test/CMakeLists.txt index 0f0becc..550b7dd 100644 --- a/bt_nodes/bt_test/CMakeLists.txt +++ b/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 diff --git a/bt_nodes/bt_test/bt_xml/extract_person_description_test.xml b/bt_nodes/bt_test/bt_xml/extract_person_description_test.xml new file mode 100644 index 0000000..112d191 --- /dev/null +++ b/bt_nodes/bt_test/bt_xml/extract_person_description_test.xml @@ -0,0 +1,42 @@ + + + + + + + + + + + + + + + + + + + + array of frames + value in meters + + [depth_nearest, left_to_right] + + + + + + + + + + + + + \ No newline at end of file diff --git a/bt_nodes/bt_test/src/extract_person_description_test.cpp b/bt_nodes/bt_test/src/extract_person_description_test.cpp new file mode 100644 index 0000000..6631336 --- /dev/null +++ b/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 +#include + +#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( + "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(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; +} diff --git a/bt_nodes/perception/CMakeLists.txt b/bt_nodes/perception/CMakeLists.txt index c839df6..d672546 100644 --- a/bt_nodes/perception/CMakeLists.txt +++ b/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) diff --git a/bt_nodes/perception/include/perception/extract_person_description.hpp b/bt_nodes/perception/include/perception/extract_person_description.hpp new file mode 100644 index 0000000..fd0bdbb --- /dev/null +++ b/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 +#include +#include +#include + +#include +#include +#include + +#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("person_id"), + BT::InputPort("interest"), + BT::OutputPort("description")}); + } + +private: + std::shared_ptr node_; + + std::string person_id_; + std::string interest_; + std::string description_; + + std::map> gestures_; + std::map pose_names_; +}; + +} // namespace perception + +#endif // PERCEPTION__EXTRACT_PERSON_DESCRIPTION_HPP_ diff --git a/bt_nodes/perception/src/perception/extract_person_description.cpp b/bt_nodes/perception/src/perception/extract_person_description.cpp new file mode 100644 index 0000000..5881891 --- /dev/null +++ b/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 +#include +#include + +#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( + "ExtractPersonDescription"); +}