GPSR: Adjusting counting people

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

bt_nodes/bt_test/bt_xml/set_blackboard_test.xml

@@ -0,0 +1,8 @@
+<root>
+    <BehaviorTree ID="GPSR">
+        <Sequence>
+            <Action ID="SetBlackboardInt" output_key="result_int" value="42"/>
+            <Action ID="SetBlackboard" output_key="result" value="{result_int}"/>
+        </Sequence>
+    </BehaviorTree> 
+</root>

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 <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"
+#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<rclcpp_cascade_lifecycle::CascadeLifecycleNode>(
+      "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<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();
+  }
+
+  std::string result;
+  blackboard->get("result", result);
+
+  std::cout << "Finished " << result << std::endl;
+
+  rclcpp::shutdown();
+  return 0;
+}

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)

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

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);
 

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<configuration::SetBlackboardInt>("SetBlackboardInt");
+}

bt_nodes/perception/CMakeLists.txt

@@ -47,7 +47,7 @@ set(dependencies
   perception_system_interfaces
   sensor_msgs
   std_srvs
-  vqa_interfaces
+  # vqa_interfaces
   # backward_ros
 )
 

robocup_bringup/bt_xml/gpsr.xml

@@ -2,6 +2,7 @@
     <BehaviorTree ID="GPSR">
         <Sequence>
             <Action ID="SetupGPSR" plugins="{plugins}"/>
+            <SetBlackboard output_key="cam_frame" value="head_front_camera_color_optical_frame"/>
             <!-- <Action ID="SetStartPosition" frame_name="instruction point" /> -->
             <Action ID="SetWp"/>
 

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,