diff --git a/bt_nodes/bt_test/CMakeLists.txt b/bt_nodes/bt_test/CMakeLists.txt
index 550b7dd..c8926cf 100644
--- a/bt_nodes/bt_test/CMakeLists.txt
+++ b/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
diff --git a/bt_nodes/bt_test/bt_xml/music_test.xml b/bt_nodes/bt_test/bt_xml/music_test.xml
new file mode 100644
index 0000000..24eb4f1
--- /dev/null
+++ b/bt_nodes/bt_test/bt_xml/music_test.xml
@@ -0,0 +1,11 @@
+
+
+
+
+
+
+
+
+
+
+
diff --git a/bt_nodes/bt_test/src/gpsr_answerquiz_test.cpp b/bt_nodes/bt_test/src/gpsr_answerquiz_test.cpp
index 82beea8..664241c 100644
--- a/bt_nodes/bt_test/src/gpsr_answerquiz_test.cpp
+++ b/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);
diff --git a/bt_nodes/bt_test/src/gpsr_countobject_test.cpp b/bt_nodes/bt_test/src/gpsr_countobject_test.cpp
index da09ad4..95a24b4 100644
--- a/bt_nodes/bt_test/src/gpsr_countobject_test.cpp
+++ b/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(tree, 10, 2666, 2667);
diff --git a/bt_nodes/bt_test/src/gpsr_findobject_test.cpp b/bt_nodes/bt_test/src/gpsr_findobject_test.cpp
index fc74f81..6de9673 100644
--- a/bt_nodes/bt_test/src/gpsr_findobject_test.cpp
+++ b/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(tree, 10, 2666,
diff --git a/bt_nodes/bt_test/src/music_test.cpp b/bt_nodes/bt_test/src/music_test.cpp
new file mode 100644
index 0000000..8be6f80
--- /dev/null
+++ b/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
+#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);
+
+ auto node = std::make_shared(
+ "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(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;
+}
diff --git a/bt_nodes/configuration/src/configuration/Setup_gpsr.cpp b/bt_nodes/configuration/src/configuration/Setup_gpsr.cpp
index 53c83a3..d6322e0 100644
--- a/bt_nodes/configuration/src/configuration/Setup_gpsr.cpp
+++ b/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);
diff --git a/bt_nodes/hri/include/hri/start_music.hpp b/bt_nodes/hri/include/hri/start_music.hpp
index abe2f90..0bfbec4 100644
--- a/bt_nodes/hri/include/hri/start_music.hpp
+++ b/bt_nodes/hri/include/hri/start_music.hpp
@@ -29,7 +29,7 @@ namespace hri
{
class StartMusic
- : public hri::BtServiceNode
{
public:
@@ -44,11 +44,13 @@ class StartMusic
{
return BT::PortsList({
BT::InputPort("audio", "elevator", "audio to play"),
+ BT::InputPort("loop", false, "loop the audio"),
});
}
private:
std::string audio_;
+ bool loop_;
};
} // namespace hri
diff --git a/bt_nodes/hri/include/hri/stop_music.hpp b/bt_nodes/hri/include/hri/stop_music.hpp
index 9aea473..b1abd41 100644
--- a/bt_nodes/hri/include/hri/stop_music.hpp
+++ b/bt_nodes/hri/include/hri/stop_music.hpp
@@ -29,7 +29,7 @@ namespace hri
{
class StopMusic
- : public hri::BtServiceNode
{
public:
diff --git a/bt_nodes/hri/src/hri/start_music.cpp b/bt_nodes/hri/src/hri/start_music.cpp
index daa62f6..b53fdcf 100644
--- a/bt_nodes/hri/src/hri/start_music.cpp
+++ b/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
@@ -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(
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(
- name, "/stop_music", config);
+ return std::make_unique(
+ name, "/music_play", config);
};
- factory.registerBuilder("StopMusic", builder);
+ factory.registerBuilder("StartMusic", builder);
}
diff --git a/bt_nodes/hri/src/hri/stop_music.cpp b/bt_nodes/hri/src/hri/stop_music.cpp
index e69de29..2d48572 100644
--- a/bt_nodes/hri/src/hri/stop_music.cpp
+++ b/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
+
+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(
+ 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(
+ name, "/music_stop", config);
+ };
+
+ factory.registerBuilder("StopMusic", builder);
+}
diff --git a/bt_nodes/perception/src/perception/IsDetected.cpp b/bt_nodes/perception/src/perception/IsDetected.cpp
index 45ea0ee..274c089 100644
--- a/bt_nodes/perception/src/perception/IsDetected.cpp
+++ b/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
diff --git a/robocup_bringup/bt_xml/gpsr.xml b/robocup_bringup/bt_xml/gpsr.xml
index 59ec427..25abb8c 100644
--- a/robocup_bringup/bt_xml/gpsr.xml
+++ b/robocup_bringup/bt_xml/gpsr.xml
@@ -18,7 +18,9 @@
+
+
diff --git a/robocup_bringup/launch/dialog.launch.py b/robocup_bringup/launch/dialog.launch.py
index 0eee13a..47df083 100644
--- a/robocup_bringup/launch/dialog.launch.py
+++ b/robocup_bringup/launch/dialog.launch.py
@@ -86,6 +86,11 @@ 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)
@@ -93,5 +98,6 @@ def generate_launch_description():
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