Merge pull request #40 from igonzf/deploy

Full Deploy of GPSR
CoreSenseEU · Jul 15, 2024 · 6386d0b · 6386d0b
2 parents e8c6b44 + 0a76522
commit 6386d0b
Show file tree

Hide file tree

Showing 71 changed files with 3,493 additions and 100 deletions.
diff --git a/bt_nodes/bt_test/CMakeLists.txt b/bt_nodes/bt_test/CMakeLists.txt
@@ -159,6 +159,51 @@ add_executable(rotate_test src/rotate_test.cpp)
 ament_target_dependencies(rotate_test ${dependencies})
 target_link_libraries(rotate_test motion::rotate_bt_node)
 
+# Change perception model test
+add_executable(get_perception_model_test src/get_perception_model_test.cpp)
+ament_target_dependencies(get_perception_model_test ${dependencies})
+target_link_libraries(get_perception_model_test
+  configuration::get_perception_model_bt_node
+)
+
+add_executable(set_perception_model_test src/set_perception_model_test.cpp)
+ament_target_dependencies(set_perception_model_test ${dependencies})
+target_link_libraries(set_perception_model_test
+  configuration::set_perception_model_bt_node
+)
+
+# music tests
+add_executable(music_test src/music_test.cpp)
+ament_target_dependencies(music_test ${dependencies})
+target_link_libraries(music_test 
+  hri::start_music_bt_node
+  hri::stop_music_bt_node
+  configuration::sleep_bt_node
+)
+
+# motion action tests
+add_executable(torsoheight_test src/torsoheight_test.cpp)
+ament_target_dependencies(torsoheight_test ${dependencies})
+target_link_libraries(torsoheight_test
+  motion::set_torso_height_bt_node
+)
+
+add_executable(headposition_test src/headposition_test.cpp)
+ament_target_dependencies(headposition_test ${dependencies})
+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})
@@ -190,8 +235,8 @@ configuration::setup_gpsr_bt_node
 )
 
 list(APPEND GPSR_TESTS
-  gpsr_answerquiz_test gpsr_countobject_test gpsr_findperson_test gpsr_followperson_test
-  gpsr_guideperson_test gpsr_moveto_test gpsr_offerobject_test gpsr_speak_test
+  gpsr_answerquiz_test gpsr_countobject_test gpsr_findperson_test gpsr_findpersonname_test gpsr_followperson_test
+  gpsr_guideperson_test gpsr_moveto_test gpsr_offerobject_test gpsr_speak_test gpsr_findobject_test
 )
 
 # gpsr tests
@@ -223,6 +268,7 @@ foreach(test ${GPSR_TESTS})
     motion::goal_publisher_bt_node
     configuration::deferred_bt_node
     configuration::setup_gpsr_bt_node
+    configuration::set_perception_model_bt_node
   )
 endforeach()
 
@@ -256,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})
@@ -340,15 +392,26 @@ install(TARGETS
   find_person_test
   count_people_test
   rotate_test
+  get_perception_model_test
+  set_perception_model_test
+  torsoheight_test
+  headposition_test
+  filter_object_test
+  extract_person_description_test
+  music_test
+  set_blackboard_test
+
   gpsr_test
   gpsr_answerquiz_test
   gpsr_countobject_test
   gpsr_findperson_test
+  gpsr_findpersonname_test
   gpsr_followperson_test
   gpsr_guideperson_test
   gpsr_moveto_test
   gpsr_offerobject_test
   gpsr_speak_test
+  gpsr_findobject_test
 
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib

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
@@ -0,0 +1,44 @@
+<?xml version="1.0"?>
+<root main_tree_to_execute="BehaviorTree">
+    <!-- ////////// -->
+    <BehaviorTree ID="BehaviorTree">
+        <Repeat num_cycles="100">
+            <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>
+        </Repeat>
+    </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>
diff --git a/bt_nodes/bt_test/bt_xml/filter_object_test.xml b/bt_nodes/bt_test/bt_xml/filter_object_test.xml
@@ -0,0 +1,20 @@
+<?xml version="1.0"?>
+<root main_tree_to_execute="BehaviorTree">
+    <!-- ////////// -->
+    <BehaviorTree ID="BehaviorTree">
+        <Sequence>
+            <Action ID="FilterObject" frames="{frames}" size="{size}" weight="{weight}" class="{class}" filtered_object="{object}" objects_count="{objects_count}"/>
+        </Sequence>
+    </BehaviorTree>
+    <!-- ////////// -->
+    <TreeNodesModel>
+        <Action ID="FilterObject">
+            <input_port name ="frames"></input_port>
+            <input_port default="unknown" name="size">size</input_port>
+            <input_port default="unknown" name="weight">weight</input_port>
+            <input_port default="unknown" name="class">class</input_port>
+            <output_port name="filtered_object"></output_port>
+        </Action>
+    </TreeNodesModel>
+    <!-- ////////// -->
+</root>
diff --git a/bt_nodes/bt_test/bt_xml/get_perception_model_test.xml b/bt_nodes/bt_test/bt_xml/get_perception_model_test.xml
@@ -0,0 +1,12 @@
+<?xml version="1.0"?>
+<root main_tree_to_execute="BehaviorTree">
+    <!-- ////////// -->
+    <BehaviorTree ID="BehaviorTree">
+        <Action ID="GetPerceptionModel"/>
+    </BehaviorTree>
+    <!-- ////////// -->
+    <TreeNodesModel>
+        <Action ID="GetPerceptionModel"/>
+    </TreeNodesModel>
+    <!-- ////////// -->
+</root>
diff --git a/bt_nodes/bt_test/bt_xml/gpsr_findobject_test.xml b/bt_nodes/bt_test/bt_xml/gpsr_findobject_test.xml
@@ -0,0 +1,8 @@
+<root>
+    <BehaviorTree ID="GPSR">
+        <Sequence>
+            <Action ID="SetupGPSR" plugins="{plugins}"/>
+            <Action ID="Deferred" bt_pkg="gpsr_planning" rel_path="bt_xml/find_object.xml" plugins="{plugins}"/>
+        </Sequence>
+    </BehaviorTree> 
+</root>
diff --git a/bt_nodes/bt_test/bt_xml/gpsr_findpersonname_test.xml b/bt_nodes/bt_test/bt_xml/gpsr_findpersonname_test.xml
@@ -0,0 +1,8 @@
+<root>
+    <BehaviorTree ID="GPSR">
+        <Sequence>
+            <Action ID="SetupGPSR" plugins="{plugins}"/>
+            <Action ID="Deferred" bt_pkg="gpsr_planning" rel_path="bt_xml/find_person_by_name.xml" plugins="{plugins}"/>
+        </Sequence>
+    </BehaviorTree> 
+</root>
diff --git a/bt_nodes/bt_test/bt_xml/headposition_test.xml b/bt_nodes/bt_test/bt_xml/headposition_test.xml
@@ -0,0 +1,12 @@
+<?xml version="1.0"?>
+<root main_tree_to_execute="BehaviorTree">
+    <!-- ////////// -->
+    <BehaviorTree ID="BehaviorTree">
+        <Action ID="SetHeadJointPosition" vertical="-0.5"/>
+    </BehaviorTree>
+    <!-- ////////// -->
+    <TreeNodesModel>
+        <Action ID="SetHeadJointPosition"/>
+    </TreeNodesModel>
+    <!-- ////////// -->
+</root>
diff --git a/bt_nodes/bt_test/bt_xml/music_test.xml b/bt_nodes/bt_test/bt_xml/music_test.xml
@@ -0,0 +1,11 @@
+<?xml version="1.0"?>
+<root main_tree_to_execute="BehaviorTree">
+    <!-- ////////// -->
+    <BehaviorTree ID="BehaviorTree">
+        <Sequence>
+            <Action ID="StartMusic" audio="elevator"/>
+            <Action ID="Sleep" time="5.0" />
+            <Action ID="StopMusic"/>
+        </Sequence>
+    </BehaviorTree>
+</root>
diff --git a/bt_nodes/bt_test/bt_xml/set_blackboard_test.xml b/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>
diff --git a/bt_nodes/bt_test/bt_xml/set_perception_model_test.xml b/bt_nodes/bt_test/bt_xml/set_perception_model_test.xml
@@ -0,0 +1,12 @@
+<?xml version="1.0"?>
+<root main_tree_to_execute="BehaviorTree">
+    <!-- ////////// -->
+    <BehaviorTree ID="BehaviorTree">
+        <Action ID="SetPerceptionModel" model_name="yolov8m-pose.pt"/>
+    </BehaviorTree>
+    <!-- ////////// -->
+    <TreeNodesModel>
+        <Action ID="SetPerceptionModel"/>
+    </TreeNodesModel>
+    <!-- ////////// -->
+</root>
diff --git a/bt_nodes/bt_test/bt_xml/torsoheight_test.xml b/bt_nodes/bt_test/bt_xml/torsoheight_test.xml
@@ -0,0 +1,12 @@
+<?xml version="1.0"?>
+<root main_tree_to_execute="BehaviorTree">
+    <!-- ////////// -->
+    <BehaviorTree ID="BehaviorTree">
+        <Action ID="SetTorsoHeight" height="0.2"/>
+    </BehaviorTree>
+    <!-- ////////// -->
+    <TreeNodesModel>
+        <Action ID="SetTorsoHeight"/>
+    </TreeNodesModel>
+    <!-- ////////// -->
+</root>
diff --git a/bt_nodes/bt_test/src/extract_person_description_test.cpp b/bt_nodes/bt_test/src/extract_person_description_test.cpp
@@ -0,0 +1,77 @@
+// 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;
+}