From 34c2d5753a6a5f66d7090c7492dbe21278053b0e Mon Sep 17 00:00:00 2001 From: igonzf Date: Thu, 11 Jul 2024 13:46:30 +0200 Subject: [PATCH] GPSR: Adjusting counting people --- bt_nodes/bt_test/CMakeLists.txt | 7 ++ .../bt_test/bt_xml/set_blackboard_test.xml | 8 ++ bt_nodes/bt_test/src/set_blackboard_test.cpp | 76 +++++++++++++++++++ bt_nodes/configuration/CMakeLists.txt | 3 + .../configuration/set_blackboard_int.hpp | 52 +++++++++++++ .../src/configuration/Setup_gpsr.cpp | 2 + .../src/configuration/set_blackboard_int.cpp | 20 +++++ bt_nodes/perception/CMakeLists.txt | 2 +- robocup_bringup/bt_xml/gpsr.xml | 1 + robocup_bringup/launch/dialog.launch.py | 2 +- 10 files changed, 171 insertions(+), 2 deletions(-) create mode 100644 bt_nodes/bt_test/bt_xml/set_blackboard_test.xml create mode 100644 bt_nodes/bt_test/src/set_blackboard_test.cpp create mode 100644 bt_nodes/configuration/include/configuration/set_blackboard_int.hpp create mode 100644 bt_nodes/configuration/src/configuration/set_blackboard_int.cpp diff --git a/bt_nodes/bt_test/CMakeLists.txt b/bt_nodes/bt_test/CMakeLists.txt index c568805..28aec6f 100644 --- a/bt_nodes/bt_test/CMakeLists.txt +++ b/bt_nodes/bt_test/CMakeLists.txt @@ -302,6 +302,12 @@ target_link_libraries(follow_entity_test # hri::speak_bt_node # hri::dialogConfirmation_bt_node ) + +# SetBlackboardInt test +add_executable(set_blackboard_test src/set_blackboard_test.cpp) +ament_target_dependencies(set_blackboard_test ${dependencies}) +target_link_libraries(set_blackboard_test configuration::set_blackboard_int_bt_node) + # FilterEntity test add_executable(filter_entity_test src/filter_entity_test.cpp) ament_target_dependencies(filter_entity_test ${dependencies}) @@ -393,6 +399,7 @@ install(TARGETS filter_object_test extract_person_description_test music_test + set_blackboard_test gpsr_test gpsr_answerquiz_test diff --git a/bt_nodes/bt_test/bt_xml/set_blackboard_test.xml b/bt_nodes/bt_test/bt_xml/set_blackboard_test.xml new file mode 100644 index 0000000..be262e4 --- /dev/null +++ b/bt_nodes/bt_test/bt_xml/set_blackboard_test.xml @@ -0,0 +1,8 @@ + + + + + + + + \ No newline at end of file diff --git a/bt_nodes/bt_test/src/set_blackboard_test.cpp b/bt_nodes/bt_test/src/set_blackboard_test.cpp new file mode 100644 index 0000000..f304ff8 --- /dev/null +++ b/bt_nodes/bt_test/src/set_blackboard_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" +#include "tf2_ros/buffer.h" +#include "tf2_ros/transform_listener.h" +#include "tf2_ros/transform_broadcaster.h" + +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( + "setblackboard_test", options); + + BT::BehaviorTreeFactory factory; + BT::SharedLibrary loader; + + factory.registerFromPlugin(loader.getOSName("set_blackboard_int_bt_node")); + + std::string pkgpath = ament_index_cpp::get_package_share_directory("bt_test"); + std::string xml_file = pkgpath + "/bt_xml/set_blackboard_test.xml"; + + auto blackboard = BT::Blackboard::create(); + blackboard->set("node", node); + 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(); + } + + std::string result; + blackboard->get("result", result); + + std::cout << "Finished " << result << std::endl; + + rclcpp::shutdown(); + return 0; +} diff --git a/bt_nodes/configuration/CMakeLists.txt b/bt_nodes/configuration/CMakeLists.txt index 7d8fbf1..b38a213 100644 --- a/bt_nodes/configuration/CMakeLists.txt +++ b/bt_nodes/configuration/CMakeLists.txt @@ -85,6 +85,9 @@ list(APPEND plugin_libs publish_tf_bt_node) add_library(sleep_bt_node SHARED src/configuration/sleep.cpp) list(APPEND plugin_libs sleep_bt_node) +add_library(set_blackboard_int_bt_node SHARED src/configuration/set_blackboard_int.cpp) +list(APPEND plugin_libs set_blackboard_int_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/configuration/include/configuration/set_blackboard_int.hpp b/bt_nodes/configuration/include/configuration/set_blackboard_int.hpp new file mode 100644 index 0000000..a575fa6 --- /dev/null +++ b/bt_nodes/configuration/include/configuration/set_blackboard_int.hpp @@ -0,0 +1,52 @@ +/* Copyright (C) 2018-2020 Davide Faconti, Eurecat - All Rights Reserved +* +* Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), +* to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, +* and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: +* The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. +* +* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, +* WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. +*/ + +#ifndef CONFIGURATION__SET_BLACKBOARD_INT_H +#define CONFIGURATION__SET_BLACKBOARD_INT_H + +#include "behaviortree_cpp_v3/action_node.h" +#include "behaviortree_cpp_v3/bt_factory.h" + +namespace configuration +{ +class SetBlackboardInt : public BT::SyncActionNode +{ +public: + SetBlackboardInt(const std::string& name, const BT::NodeConfiguration& config) : + SyncActionNode(name, config) + { + setRegistrationID("SetBlackboard"); + } + + static BT::PortsList providedPorts() + { + return {BT::InputPort("value", "Value to be written int othe output_key"), + BT::InputPort("output_key", "Key where the value will be written")}; + } + +private: + virtual BT::NodeStatus tick() override + { + std::string output_key; + getInput("output_key", output_key); + + int value; + getInput("value", value); + + config().blackboard->set(output_key, value); + + return BT::NodeStatus::SUCCESS; + } +}; +} // namespace BT + +#endif \ No newline at end of file diff --git a/bt_nodes/configuration/src/configuration/Setup_gpsr.cpp b/bt_nodes/configuration/src/configuration/Setup_gpsr.cpp index d6322e0..ca88071 100644 --- a/bt_nodes/configuration/src/configuration/Setup_gpsr.cpp +++ b/bt_nodes/configuration/src/configuration/Setup_gpsr.cpp @@ -70,6 +70,8 @@ BT::NodeStatus SetupGPSR::tick() { plugins.push_back("filter_prev_detections_bt_node"); plugins.push_back("extract_person_description_bt_node"); plugins.push_back("sleep_bt_node"); + plugins.push_back("count_people_bt_node"); + plugins.push_back("set_blackboard_int_bt_node"); setOutput("plugins", plugins); diff --git a/bt_nodes/configuration/src/configuration/set_blackboard_int.cpp b/bt_nodes/configuration/src/configuration/set_blackboard_int.cpp new file mode 100644 index 0000000..1db8f4a --- /dev/null +++ b/bt_nodes/configuration/src/configuration/set_blackboard_int.cpp @@ -0,0 +1,20 @@ +// 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 "configuration/set_blackboard_int.hpp" + +BT_REGISTER_NODES(factory) +{ + factory.registerNodeType("SetBlackboardInt"); +} \ No newline at end of file diff --git a/bt_nodes/perception/CMakeLists.txt b/bt_nodes/perception/CMakeLists.txt index db9f415..e7dfc2b 100644 --- a/bt_nodes/perception/CMakeLists.txt +++ b/bt_nodes/perception/CMakeLists.txt @@ -47,7 +47,7 @@ set(dependencies perception_system_interfaces sensor_msgs std_srvs - vqa_interfaces + # vqa_interfaces # backward_ros ) diff --git a/robocup_bringup/bt_xml/gpsr.xml b/robocup_bringup/bt_xml/gpsr.xml index 3315c6c..26bf927 100644 --- a/robocup_bringup/bt_xml/gpsr.xml +++ b/robocup_bringup/bt_xml/gpsr.xml @@ -2,6 +2,7 @@ + diff --git a/robocup_bringup/launch/dialog.launch.py b/robocup_bringup/launch/dialog.launch.py index 77d49b8..fcae8ff 100644 --- a/robocup_bringup/launch/dialog.launch.py +++ b/robocup_bringup/launch/dialog.launch.py @@ -45,7 +45,7 @@ def generate_launch_description(): llama_cmd = create_llama_launch( n_ctx=2048, n_batch=256, - n_gpu_layers=25, + n_gpu_layers=23, n_threads=4, n_predict=-1,