diff --git a/bt_nodes/configuration/CMakeLists.txt b/bt_nodes/configuration/CMakeLists.txt index 6d9388c..a122381 100644 --- a/bt_nodes/configuration/CMakeLists.txt +++ b/bt_nodes/configuration/CMakeLists.txt @@ -68,6 +68,9 @@ list(APPEND plugin_libs init_protected_queue_bt_node) add_library(publish_tf_bt_node SHARED src/configuration/publish_tf.cpp) 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) + foreach(bt_plugin ${plugin_libs}) ament_target_dependencies(${bt_plugin} ${dependencies}) target_compile_definitions(${bt_plugin} PRIVATE BT_PLUGIN_EXPORT) diff --git a/bt_nodes/configuration/include/configuration/sleep.hpp b/bt_nodes/configuration/include/configuration/sleep.hpp new file mode 100644 index 0000000..7e46be4 --- /dev/null +++ b/bt_nodes/configuration/include/configuration/sleep.hpp @@ -0,0 +1,60 @@ +// 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. + +#ifndef CONFIGURATION__SLEEP_HPP_ +#define CONFIGURATION__SLEEP_HPP_ + +#include + +#include +#include +#include + +#include "behaviortree_cpp_v3/behavior_tree.h" +#include "behaviortree_cpp_v3/bt_factory.h" + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_cascade_lifecycle/rclcpp_cascade_lifecycle.hpp" + +#include "rclcpp_action/rclcpp_action.hpp" + +namespace configuration +{ + +class Sleep : public BT::ActionNodeBase +{ +public: + explicit Sleep(const std::string & xml_tag_name, const BT::NodeConfiguration & conf); + + void halt(); + BT::NodeStatus tick(); + + static BT::PortsList providedPorts() + { + return BT::PortsList( + { + BT::InputPort("time") + }); + } + +private: + std::shared_ptr node_; + + double time_; + std::chrono::time_point start_time_; +}; + +} // namespace configuration + +#endif // CONFIGURATION__SLEEP_HPP_ diff --git a/bt_nodes/configuration/src/configuration/sleep.cpp b/bt_nodes/configuration/src/configuration/sleep.cpp new file mode 100644 index 0000000..a8464ff --- /dev/null +++ b/bt_nodes/configuration/src/configuration/sleep.cpp @@ -0,0 +1,55 @@ +// 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/sleep.hpp" + +namespace configuration +{ + +Sleep::Sleep(const std::string & xml_tag_name, const BT::NodeConfiguration & conf) +: BT::ActionNodeBase(xml_tag_name, conf) +{ + config().blackboard->get("node", node_); +} + +BT::NodeStatus Sleep::tick() +{ + RCLCPP_INFO(node_->get_logger(), "[Sleep] ticked"); + + getInput("time", time_); + + if (status() == BT::NodeStatus::IDLE) { + start_time_ = std::chrono::high_resolution_clock::now(); + return BT::NodeStatus::RUNNING; + } + + auto end_time = std::chrono::high_resolution_clock::now(); + auto elapsed_time = std::chrono::duration_cast(end_time - start_time_).count(); + if (elapsed_time < time_) { + return BT::NodeStatus::RUNNING; + } else { + return BT::NodeStatus::SUCCESS; + } +} + +void Sleep::halt() +{ + RCLCPP_INFO(node_->get_logger(), "[Sleep] halted"); +} + +} // namespace navigation + +BT_REGISTER_NODES(factory) { + factory.registerNodeType("Sleep"); +} diff --git a/bt_nodes/hri/CMakeLists.txt b/bt_nodes/hri/CMakeLists.txt index 91a623c..26fba6a 100644 --- a/bt_nodes/hri/CMakeLists.txt +++ b/bt_nodes/hri/CMakeLists.txt @@ -17,7 +17,9 @@ find_package(llama_msgs REQUIRED) find_package(moveit_msgs REQUIRED) find_package(shape_msgs REQUIRED) find_package(std_msgs REQUIRED) +find_package(std_srvs REQUIRED) find_package(gpsr_msgs REQUIRED) +find_package(audio_common_msgs REQUIRED) set(CMAKE_CXX_STANDARD 17) set(dependencies @@ -32,6 +34,8 @@ set(dependencies shape_msgs std_msgs gpsr_msgs + std_srvs + audio_common_msgs ) include_directories(include) @@ -60,6 +64,12 @@ list(APPEND plugin_libs choose_from_classes_bt_node) add_library(command_planning_bt_node SHARED src/hri/CommandPlanning.cpp) list(APPEND plugin_libs command_planning_bt_node) +add_library(start_music_bt_node SHARED src/hri/start_music.cpp) +list(APPEND plugin_libs start_music_bt_node) + +add_library(stop_music_bt_node SHARED src/hri/stop_music.cpp) +list(APPEND plugin_libs stop_music_bt_node) + foreach(bt_plugin ${plugin_libs}) ament_target_dependencies(${bt_plugin} ${dependencies}) target_compile_definitions(${bt_plugin} PRIVATE BT_PLUGIN_EXPORT) diff --git a/bt_nodes/hri/include/hri/start_music.hpp b/bt_nodes/hri/include/hri/start_music.hpp new file mode 100644 index 0000000..abe2f90 --- /dev/null +++ b/bt_nodes/hri/include/hri/start_music.hpp @@ -0,0 +1,56 @@ +// 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 andGO2OBJECT +// limitations under the License. + +#ifndef HRI__START_MUSIC_HPP_ +#define HRI__START_MUSIC_HPP_ + +#include + +#include "behaviortree_cpp_v3/behavior_tree.h" +#include "behaviortree_cpp_v3/bt_factory.h" +#include "hri/bt_service_node.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_cascade_lifecycle/rclcpp_cascade_lifecycle.hpp" +#include "audio_common_msgs/srv/music_play.hpp" + + +namespace hri +{ + +class StartMusic + : public hri::BtServiceNode +{ +public: + explicit StartMusic( + const std::string & xml_tag_name, const std::string & action_name, + const BT::NodeConfiguration & conf); + + void on_tick() override; + void on_result() override; + + static BT::PortsList providedPorts() + { + return BT::PortsList({ + BT::InputPort("audio", "elevator", "audio to play"), + }); + } + +private: + std::string audio_; +}; + +} // namespace hri + +#endif // HRI__START_MUSIC_HPP_ diff --git a/bt_nodes/hri/include/hri/stop_music.hpp b/bt_nodes/hri/include/hri/stop_music.hpp new file mode 100644 index 0000000..9aea473 --- /dev/null +++ b/bt_nodes/hri/include/hri/stop_music.hpp @@ -0,0 +1,52 @@ +// 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 andGO2OBJECT +// limitations under the License. + +#ifndef HRI__STOP_MUSIC_HPP_ +#define HRI__STOP_MUSIC_HPP_ + +#include + +#include "behaviortree_cpp_v3/behavior_tree.h" +#include "behaviortree_cpp_v3/bt_factory.h" +#include "hri/bt_service_node.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_cascade_lifecycle/rclcpp_cascade_lifecycle.hpp" +#include "std_srvs/srv/trigger.hpp" + + +namespace hri +{ + +class StopMusic + : public hri::BtServiceNode +{ +public: + explicit StopMusic( + const std::string & xml_tag_name, const std::string & action_name, + const BT::NodeConfiguration & conf); + + void on_tick() override; + void on_result() override; + + static BT::PortsList providedPorts() + { + return BT::PortsList({ + }); + } +}; + +} // namespace hri + +#endif // HRI__STOP_MUSIC_HPP_ diff --git a/bt_nodes/hri/src/hri/start_music.cpp b/bt_nodes/hri/src/hri/start_music.cpp new file mode 100644 index 0000000..daa62f6 --- /dev/null +++ b/bt_nodes/hri/src/hri/start_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) +: configuration::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, "/stop_music", config); + }; + + factory.registerBuilder("StopMusic", builder); +} diff --git a/bt_nodes/motion/src/motion/base/empty b/bt_nodes/hri/src/hri/stop_music.cpp similarity index 100% rename from bt_nodes/motion/src/motion/base/empty rename to bt_nodes/hri/src/hri/stop_music.cpp