Sleep and Music BT Nodes

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)

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 <tf2_ros/buffer.h>
+
+#include <memory>
+#include <string>
+#include <chrono>
+
+#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<double>("time")
+    });
+  }
+
+private:
+    std::shared_ptr<rclcpp_cascade_lifecycle::CascadeLifecycleNode> node_;
+
+    double time_;
+    std::chrono::time_point<std::chrono::high_resolution_clock> start_time_;
+};
+
+}  // namespace configuration
+
+#endif  // CONFIGURATION__SLEEP_HPP_

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<std::chrono::seconds>(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<configuration::Sleep>("Sleep");
+}

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)

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 <string>
+
+#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<yolov8_msgs::srv::ChangeModel,
+  rclcpp_cascade_lifecycle::CascadeLifecycleNode>
+{
+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<std::string>("audio", "elevator", "audio to play"),
+    });
+  }
+
+private:
+  std::string audio_;
+};
+
+}  // namespace hri
+
+#endif  // HRI__START_MUSIC_HPP_

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 <string>
+
+#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<yolov8_msgs::srv::ChangeModel,
+  rclcpp_cascade_lifecycle::CascadeLifecycleNode>
+{
+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_

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 <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)
+: configuration::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, "/stop_music", config);
+    };
+
+  factory.registerBuilder<hri::StopMusic>("StopMusic", builder);
+}