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