Added music for GPSR

bt_nodes/bt_test/CMakeLists.txt

@@ -167,6 +167,15 @@ 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})
@@ -378,6 +387,7 @@ install(TARGETS
   headposition_test
   filter_object_test
   extract_person_description_test
+  music_test
 
   gpsr_test
   gpsr_answerquiz_test

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>

bt_nodes/bt_test/src/gpsr_answerquiz_test.cpp

@@ -43,6 +43,7 @@ int main(int argc, char *argv[]) {
 
   auto blackboard = BT::Blackboard::create();
   blackboard->set("node", node);
+  blackboard->set("text_value", "What day is tomorrow?");
 
   BT::Tree tree = factory.createTreeFromFile(xml_file, blackboard);
 

bt_nodes/bt_test/src/gpsr_countobject_test.cpp

@@ -43,7 +43,7 @@ int main(int argc, char *argv[]) {
 
   auto blackboard = BT::Blackboard::create();
   blackboard->set("node", node);
-  blackboard->set("object_type", "cup");
+  blackboard->set("class", "fruit");
   BT::Tree tree = factory.createTreeFromFile(xml_file, blackboard);
 
   // auto publisher_zmq = std::make_shared<BT::PublisherZMQ>(tree, 10, 2666, 2667);

bt_nodes/bt_test/src/gpsr_findobject_test.cpp

@@ -43,9 +43,9 @@ int main(int argc, char *argv[]) {
 
   auto blackboard = BT::Blackboard::create();
   blackboard->set("node", node);
-  blackboard->set("size", "unknown");
+  blackboard->set("size", "big");
   blackboard->set("weight", "unknown");
-  blackboard->set("class", "fruit");
+  blackboard->set("class", "unknown");
   BT::Tree tree = factory.createTreeFromFile(xml_file, blackboard);
 
   // auto publisher_zmq = std::make_shared<BT::PublisherZMQ>(tree, 10, 2666,

bt_nodes/bt_test/src/music_test.cpp

@@ -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.
+
+#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);
+
+  auto node = std::make_shared<rclcpp_cascade_lifecycle::CascadeLifecycleNode>(
+    "setperceptionmodel_test");
+
+  BT::BehaviorTreeFactory factory;
+  BT::SharedLibrary loader;
+
+  factory.registerFromPlugin(loader.getOSName("start_music_bt_node"));
+  factory.registerFromPlugin(loader.getOSName("stop_music_bt_node"));
+  factory.registerFromPlugin(loader.getOSName("sleep_bt_node"));
+
+  std::string pkgpath = ament_index_cpp::get_package_share_directory("bt_test");
+  std::string xml_file = pkgpath + "/bt_xml/music_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, 1666, 1667);
+
+  node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE);
+  node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE);
+
+  rclcpp::Rate rate(10);
+
+  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/configuration/src/configuration/Setup_gpsr.cpp

@@ -68,6 +68,8 @@ BT::NodeStatus SetupGPSR::tick() {
   plugins.push_back("publish_tf_bt_node");
   plugins.push_back("init_protected_queue_bt_node");
   plugins.push_back("filter_prev_detections_bt_node");
+  plugins.push_back("extract_person_description_bt_node");
+  plugins.push_back("sleep_bt_node");
 
   setOutput("plugins", plugins);
 

bt_nodes/hri/include/hri/start_music.hpp

@@ -29,7 +29,7 @@ namespace hri
 {
 
 class StartMusic
-  : public hri::BtServiceNode<yolov8_msgs::srv::ChangeModel,
+  : public hri::BtServiceNode<audio_common_msgs::srv::MusicPlay,
   rclcpp_cascade_lifecycle::CascadeLifecycleNode>
 {
 public:
@@ -44,11 +44,13 @@ class StartMusic
   {
     return BT::PortsList({
       BT::InputPort<std::string>("audio", "elevator", "audio to play"),
+      BT::InputPort<bool>("loop", false, "loop the audio"),
     });
   }
 
 private:
   std::string audio_;
+  bool loop_;
 };
 
 }  // namespace hri

bt_nodes/hri/include/hri/stop_music.hpp

@@ -29,7 +29,7 @@ namespace hri
 {
 
 class StopMusic
-  : public hri::BtServiceNode<yolov8_msgs::srv::ChangeModel,
+  : public hri::BtServiceNode<std_srvs::srv::Trigger,
   rclcpp_cascade_lifecycle::CascadeLifecycleNode>
 {
 public:

bt_nodes/hri/src/hri/start_music.cpp

@@ -12,7 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "hri/stop_music.hpp"
+#include "hri/start_music.hpp"
 
 #include <string>
 
@@ -22,28 +22,34 @@ namespace hri
 using namespace std::chrono_literals;
 using namespace std::placeholders;
 
-StopMusic::StopMusic(
+StartMusic::StartMusic(
   const std::string & xml_tag_name, const std::string & action_name,
   const BT::NodeConfiguration & conf)
-: configuration::BtServiceNode<std_srvs::srv::Trigger,
+: hri::BtServiceNode<audio_common_msgs::srv::MusicPlay,
     rclcpp_cascade_lifecycle::CascadeLifecycleNode>(
     xml_tag_name, action_name, conf)
 {
 }
 
-void StopMusic::on_tick()
+void StartMusic::on_tick()
 {
-    RCLCPP_DEBUG(node_->get_logger(), "StopMusic ticked");
+    RCLCPP_DEBUG(node_->get_logger(), "StartMusic ticked");
+
+    getInput("audio", audio_);
+    getInput("loop", loop_);
+
+    request_->audio = audio_;
+    request_->loop = loop_;
 }
 
-void StopMusic::on_result()
+void StartMusic::on_result()
 {
   if (result_.success) {
-    std::cout << "Success StopMusic" << std::endl;
+    std::cout << "Success StartMusic" << std::endl;
     setStatus(BT::NodeStatus::SUCCESS);
 
   } else {
-    std::cout << "Failure StopMusic" << std::endl;
+    std::cout << "Failure StartMusic" << std::endl;
     // setOutput("listen_text", result_.result->text);
     setStatus(BT::NodeStatus::FAILURE);
   }
@@ -55,9 +61,9 @@ void StopMusic::on_result()
 BT_REGISTER_NODES(factory)
 {
   BT::NodeBuilder builder = [](const std::string & name, const BT::NodeConfiguration & config) {
-      return std::make_unique<hri::StopMusic>(
-        name, "/stop_music", config);
+      return std::make_unique<hri::StartMusic>(
+        name, "/music_play", config);
     };
 
-  factory.registerBuilder<hri::StopMusic>("StopMusic", builder);
+  factory.registerBuilder<hri::StartMusic>("StartMusic", builder);
 }

bt_nodes/hri/src/hri/stop_music.cpp

@@ -0,0 +1,63 @@
+// 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 "hri/stop_music.hpp"
+
+#include <string>
+
+namespace hri
+{
+
+using namespace std::chrono_literals;
+using namespace std::placeholders;
+
+StopMusic::StopMusic(
+  const std::string & xml_tag_name, const std::string & action_name,
+  const BT::NodeConfiguration & conf)
+: hri::BtServiceNode<std_srvs::srv::Trigger,
+    rclcpp_cascade_lifecycle::CascadeLifecycleNode>(
+    xml_tag_name, action_name, conf)
+{
+}
+
+void StopMusic::on_tick()
+{
+    RCLCPP_DEBUG(node_->get_logger(), "StopMusic ticked");
+}
+
+void StopMusic::on_result()
+{
+  if (result_.success) {
+    std::cout << "Success StopMusic" << std::endl;
+    setStatus(BT::NodeStatus::SUCCESS);
+
+  } else {
+    std::cout << "Failure StopMusic" << std::endl;
+    // setOutput("listen_text", result_.result->text);
+    setStatus(BT::NodeStatus::FAILURE);
+  }
+}
+
+}  // namespace perception
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+BT_REGISTER_NODES(factory)
+{
+  BT::NodeBuilder builder = [](const std::string & name, const BT::NodeConfiguration & config) {
+      return std::make_unique<hri::StopMusic>(
+        name, "/music_stop", config);
+    };
+
+  factory.registerBuilder<hri::StopMusic>("StopMusic", builder);
+}

bt_nodes/perception/src/perception/IsDetected.cpp

@@ -117,6 +117,7 @@ BT::NodeStatus IsDetected::tick()
 
   // pub->publish(detections[0].image);
 
+  RCLCPP_INFO(node_->get_logger(), "[IsDetected] best detection: %s", detections[0].class_name.c_str());
   setOutput("best_detection", detections[0].class_name);
   RCLCPP_INFO(node_->get_logger(), "[IsDetected] Detections sorted");
   // implement more sorting methods

robocup_bringup/bt_xml/gpsr.xml

@@ -18,7 +18,9 @@
                     <RetryUntilSuccessful num_attempts="-1">
                         <Sequence>
                             <Action ID="Speak" say_text="Thinking about the process." param=""/>
+                            <Action ID="StartMusic" audio="elevator" loop="true" />
                             <Action ID="CommandPlanning" command="{command}" actions="{action_list}" bt_value="{bt_xml}"/>
+                            <Action ID="StopMusic" />
                             <Action ID="Speak" say_text="Then I have to do the following actions " param="{action_list}"/>
                             <Action ID="Speak" say_text="This is correct?" param=""/>
                             <Action ID="DialogConfirmation"/>

robocup_bringup/launch/dialog.launch.py

@@ -86,12 +86,18 @@ def generate_launch_description():
             {'device': -1}]
     )
 
+    music_player_node = Node(
+        package='audio_common',
+        executable='music_node',
+    )
+
     ld = LaunchDescription()
     ld.add_action(declare_model_repo_cmd)
     ld.add_action(declare_model_filename_cmd)
     ld.add_action(whisper_cmd)
     ld.add_action(llama_cmd)
     ld.add_action(audio_common_tts_node)
     ld.add_action(audio_common_player_node)
+    ld.add_action(music_player_node)
 
     return ld