speaker support (#127)

# PR Summary PR Link: INSERT-LINK-HERE Issue Link: INSERT-LINK-HERE ### Description Add a single line summary describing the purpose of this PR. ### Reviewers Tag reviewers. - Required: @JakeWendling - Optional: --- ### Changelog - publish tts and music topics when function called in tank_robot_config - make the audoi node with chatgpt and set volume stuff ### Reviewer Guide this shouldn't break anything. The speaker code isn't written safely cuz it's a seperate node so even if it breaks we don't care ### Testing #### Automatic no #### Manual https://github.com/user-attachments/assets/8ddf9ea1-64a3-4a2a-a99e-6714751ba228 ### Documentation no ### Checklist - [ ] Confirmed all tests pass on a clean build - [x] Added reviewers in Github - [x] Posted PR Summary to Discord PR's Channel - [ ] Ran uncrustify on any modified C++ files - [ ] Ran Colcon Lint for any modified CMakeLists.txt or Package.xml --------- Co-authored-by: Omega Jerry <[email protected]> Co-authored-by: JakeWendling <[email protected]> Co-authored-by: Maxx Wilson <[email protected]> Co-authored-by: MELISSA CRUZ <[email protected]>
VEXU-GHOST · Mar 6, 2025 · 2d5f742 · 2d5f742
1 parent 6e479dd
commit 2d5f742
Show file tree

Hide file tree

Showing 13 changed files with 298 additions and 8 deletions.
diff --git a/.gitignore b/.gitignore
@@ -4,6 +4,13 @@ log
 .vscode
 *.pyc
 .cache/*
+sound_effects
+*.wav
+*.mp3
+
+# Voice Models
+*.onnx
+*.onnx.json
 
 # These are symlinked from 01_Libraries (and thus duplicated)
 02_V5/ghost_pros/include/ghost_v5_interfaces/**

diff --git a/03_ROS/ghost_io_py/ghost_io_py/__init__.py b/03_ROS/ghost_io_py/ghost_io_py/__init__.py
diff --git a/03_ROS/ghost_io_py/ghost_io_py/ghost_tts.py b/03_ROS/ghost_io_py/ghost_io_py/ghost_tts.py
@@ -0,0 +1,182 @@
+#!/usr/bin/env python3
+# thanks chat, https://chatgpt.com/share/67b95b69-6e78-800c-a0a8-64583e95fa80
+import rclpy
+from rclpy.node import Node
+from std_msgs.msg import String
+import tempfile
+import wave
+import subprocess
+import os
+import random
+from pathlib import Path
+from typing import Any, Dict, Optional, List
+
+# Import PiperVoice and auto-download features from the Piper TTS library.
+from piper import PiperVoice
+from piper.download import ensure_voice_exists, find_voice, get_voices
+
+class TTSMusicNode(Node):
+    def __init__(self) -> None:
+        super().__init__('tts_music_node')
+        self.get_logger().info("Initializing TTSMusicNode with auto-download support")
+
+        # Declare parameters for PiperVoice and auto-download.
+        # Default model is set to "en_GB-alan-low"
+        self.declare_parameter("model", "en_US-ryan-low")
+        self.declare_parameter("config", "")
+        self.declare_parameter("data_dir", [str(Path.cwd())])
+        self.declare_parameter("download_dir", "")
+        self.declare_parameter("update_voices", False)
+        self.declare_parameter("use_cuda", False)
+        self.declare_parameter("music_folder", "~/VEXU_GHOST/sound_effects")
+        self.declare_parameter("volume", 10)
+
+        model: str = self.get_parameter("model").value
+        config: str = self.get_parameter("config").value
+        data_dir: List[str] = self.get_parameter("data_dir").value
+        download_dir: str = self.get_parameter("download_dir").value
+        update_voices: bool = self.get_parameter("update_voices").value
+        use_cuda: bool = self.get_parameter("use_cuda").value
+        volume: int = self.get_parameter("volume").value
+
+        if not download_dir:
+            download_dir = data_dir[0]
+            self.get_logger().info(f"No download_dir provided, defaulting to {download_dir}")
+
+        model_path: Path = Path(model)
+        if not model_path.exists():
+            self.get_logger().info(f"Model '{model}' not found locally. Initiating auto-download.")
+            try:
+                voices_info: Dict[str, Any] = get_voices(download_dir, update_voices=update_voices)
+                aliases_info: Dict[str, Any] = {}
+                for voice_info in voices_info.values():
+                    for voice_alias in voice_info.get("aliases", []):
+                        aliases_info[voice_alias] = {"_is_alias": True, **voice_info}
+                voices_info.update(aliases_info)
+                ensure_voice_exists(model, data_dir, download_dir, voices_info)
+                model, config = find_voice(model, data_dir)
+                self.get_logger().info(f"Model downloaded and resolved: {model}")
+            except Exception as e:
+                model, config = find_voice(model, data_dir)
+                self.get_logger().error(f"Auto-download failed: {e}")
+                #raise e
+        else:
+            self.get_logger().info(f"Using local model: {model}")
+
+        try:
+            self.voice: PiperVoice = PiperVoice.load(model, config_path=config, use_cuda=use_cuda)
+            self.get_logger().info("PiperVoice loaded successfully.")
+        except Exception as e:
+            self.get_logger().error(f"Failed to load PiperVoice: {e}")
+            raise e
+
+        # Define synthesis arguments.
+        # Removed "speaker_id" as it may cause invalid input error with certain models.
+        self.synthesize_args: Dict[str, Any] = {
+            "length_scale": 1.0,
+            "noise_scale": 0.667,
+            "noise_w": 0.8,
+            "sentence_silence": 0.5,
+        }
+        self.get_logger().info(f"Setting volume to: {volume}%")
+        command = (
+            "sink=$(pactl list sinks short | awk '/usb/ {print $2; exit}'); "
+            'pactl set-default-sink "$sink" && pactl set-sink-volume "$sink" ' + str(volume)+ '%'
+        )
+
+        subprocess.run(command, shell=True)
+
+        # Create subscriptions for TTS and music topics.
+        self.create_subscription(String, "/io/speaker/tts", self.tts_callback, 10)
+        self.create_subscription(String, "/io/speaker/music", self.music_callback, 10)
+
+    def tts_callback(self, msg: String) -> None:
+        """
+        Callback for TTS messages.
+        Synthesizes speech from the received text and plays the resulting WAV file.
+        """
+        text: str = msg.data.strip()
+        if not text:
+            self.get_logger().warn("Received empty TTS message; ignoring.")
+            return
+
+        self.get_logger().info(f"Received TTS message: '{text}'")
+        try:
+            # Create a temporary WAV file for synthesized audio.
+            with tempfile.NamedTemporaryFile(suffix=".wav", delete=False) as tmp_file:
+                tmp_filename: str = tmp_file.name
+
+            with wave.open(tmp_filename, "wb") as wav_file:
+                self.voice.synthesize(text, wav_file, **self.synthesize_args)
+
+            # Play the synthesized audio using an external audio player.
+            subprocess.run(["play", tmp_filename], capture_output=True, text  = True)
+        except Exception as e:
+            self.get_logger().warn(f"Error during TTS synthesis: {e}")
+
+    def music_callback(self, msg: String) -> None:
+        """
+        Callback for music messages.
+        Plays a music file from a designated folder. If the provided key matches part
+        of a filename, that file is played. Otherwise, a random file from the folder is chosen.
+        Before attempting to play, the file's existence is verified.
+        """
+        key: str = msg.data.strip().lower()
+        music_folder: str = os.path.expanduser(self.get_parameter("music_folder").value)
+
+        if not os.path.isdir(music_folder):
+            self.get_logger().warn(f"Music folder '{music_folder}' does not exist or is not a directory.")
+            return
+
+        # List all .wav files in the music folder
+        music_files: List[str] = [
+            os.path.join(music_folder, f)
+            for f in os.listdir(music_folder)
+            if f.lower().endswith(".wav") or f.lower().endswith("mp3")
+        ]
+
+        if not music_files:
+            self.get_logger().warn(f"No .wav files found in folder '{music_folder}'.")
+            return
+
+        # Find all files whose name contains the key
+        matching_files: List[str] = [
+            file for file in music_files if key in os.path.basename(file).lower()
+        ]
+
+        if len(matching_files) == 1:
+            selected_file: str = matching_files[0]
+        else:
+            if matching_files:
+                self.get_logger().warn(f"Multiple music files match key '{key}'; selecting a random one from matches.")
+                selected_file = random.choice(matching_files)
+            else:
+                self.get_logger().warn(f"Music key '{key}' not found; selecting a random file from all files.")
+                selected_file = random.choice(music_files)
+
+
+        if not os.path.isfile(selected_file):
+            self.get_logger().warn(f"Selected music file '{selected_file}' does not exist.")
+            return
+
+        self.get_logger().info(f"Playing music file: {selected_file}")
+        try:
+            subprocess.run(["play", selected_file], capture_output=True, text  = True)
+        except Exception as e:
+            self.get_logger().warn(f"Error playing music file '{selected_file}': {e}")
+
+
+def main(args: Optional[Any] = None) -> None:
+    rclpy.init(args=args)
+    node: TTSMusicNode = TTSMusicNode()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        node.get_logger().info("Shutting down TTSMusicNode.")
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()
diff --git a/03_ROS/ghost_io_py/package.xml b/03_ROS/ghost_io_py/package.xml
@@ -0,0 +1,13 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>ghost_io_py</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="[email protected]">karmanyaahm</maintainer>
+  <license>TODO: License declaration</license>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>
diff --git a/03_ROS/ghost_io_py/resource/ghost_io_py b/03_ROS/ghost_io_py/resource/ghost_io_py
diff --git a/03_ROS/ghost_io_py/setup.cfg b/03_ROS/ghost_io_py/setup.cfg
@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/ghost_io_py
+[install]
+install_scripts=$base/lib/ghost_io_py
diff --git a/03_ROS/ghost_io_py/setup.py b/03_ROS/ghost_io_py/setup.py
@@ -0,0 +1,26 @@
+from setuptools import find_packages, setup
+
+package_name = 'ghost_io_py'
+
+setup(
+    name=package_name,
+    version='0.0.0',
+    packages=find_packages(exclude=['test']),
+    data_files=[
+        ('share/ament_index/resource_index/packages',
+            ['resource/' + package_name]),
+        ('share/' + package_name, ['package.xml']),
+    ],
+    install_requires=['setuptools'],
+    zip_safe=True,
+    maintainer='karmanyaahm',
+    maintainer_email='[email protected]',
+    description='TODO: Package description',
+    license='TODO: License declaration',
+    tests_require=['pytest'],
+    entry_points={
+        'console_scripts': [
+            'ghost_tts = ghost_io_py.ghost_tts:main'
+        ],
+    },
+)
diff --git a/11_Robots/ghost_high_stakes/config/ros_config.yaml b/11_Robots/ghost_high_stakes/config/ros_config.yaml
@@ -736,4 +736,7 @@ map_ekf_node:
         0.0,  0.0,  0.0,  0.0,  0.0, 0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0, 1e-9,
       ]
 
-    dynamic_process_noise_covariance: true
+    dynamic_process_noise_covariance: true
+tts_music_node:
+  ros__parameters:
+    volume: 11  # set to 0 to turn off, set to 100 at comp
diff --git a/11_Robots/ghost_high_stakes/launch/hardware.launch.py b/11_Robots/ghost_high_stakes/launch/hardware.launch.py
@@ -116,6 +116,14 @@ def generate_launch_description():
         parameters=[ros_config_file],
     )
 
+    tts_music_node = Node(
+        package="ghost_io_py",
+        executable="ghost_tts",
+        name="tts_music_node",
+        output="screen",
+        parameters=[ros_config_file],
+    )
+
     # realsense_node = IncludeLaunchDescription(
     #     PythonLaunchDescriptionSource(
     #         os.path.join(
@@ -172,7 +180,8 @@ def generate_launch_description():
         odom_ekf_node,
         map_ekf_node,
         rplidar_node,
-        competition_state_machine_node,
         color_classifier_node,
-        color_sensor_node
+        color_sensor_node,
+        tts_music_node,
+        competition_state_machine_node,
     ])
diff --git a/11_Robots/ghost_tank/include/ghost_tank/tank_robot_plugin.hpp b/11_Robots/ghost_tank/include/ghost_tank/tank_robot_plugin.hpp
@@ -104,6 +104,12 @@ class TankRobotPlugin : public ghost_ros_interfaces::V5RobotBase
   void updateBite(std::shared_ptr<ghost_v5_interfaces::devices::JoystickDeviceData> joy_data);
   void updateGoalRush(std::shared_ptr<ghost_v5_interfaces::devices::JoystickDeviceData> joy_data);
   void updateNeutralStakeArm(std::shared_ptr<ghost_v5_interfaces::devices::JoystickDeviceData> joy_data);
+  void updateMusic(double current_time, std::shared_ptr<ghost_v5_interfaces::devices::JoystickDeviceData> joy_data);
+
+  // Output
+  void playMusic(std::string m);
+  void playTTS(std::string m);
+
 
   void resetWorldPose();
 
@@ -120,6 +126,9 @@ class TankRobotPlugin : public ghost_ros_interfaces::V5RobotBase
   rclcpp::Publisher<geometry_msgs::msg::Pose>::SharedPtr m_err_pos_pub;
   rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr m_set_pose_publisher;
 
+  rclcpp::Publisher<std_msgs::msg::String>::SharedPtr m_tts_pub;
+  rclcpp::Publisher<std_msgs::msg::String>::SharedPtr m_music_pub;
+
   rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr m_reset_ekf_pub;
   rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr m_reset_pf_pub;
 
@@ -153,7 +162,6 @@ class TankRobotPlugin : public ghost_ros_interfaces::V5RobotBase
   std::shared_ptr<TankModel> m_tank_model_ptr;
 
   // Autonomy
-  void movePointToPoint();
   std::string bt_path_;
   std::shared_ptr<TankTree> bt_;
   std::shared_ptr<TankTree> bt_interaction;

diff --git a/11_Robots/ghost_tank/package.xml b/11_Robots/ghost_tank/package.xml
@@ -14,6 +14,7 @@
   <depend>ghost_ros_interfaces</depend>
   <depend>ghost_util</depend>
   <depend>ghost_estimation</depend>
+  <depend>ghost_io_py</depend>
   <depend>geometry_msgs</depend>
   <depend>sensor_msgs</depend>
   <depend>nav_msgs</depend>

diff --git a/11_Robots/ghost_tank/src/tank_robot_plugin.cpp b/11_Robots/ghost_tank/src/tank_robot_plugin.cpp
@@ -164,6 +164,9 @@ void TankRobotPlugin::initROSComms()
   node_ptr_->declare_parameter("tank_robot_plugin.err_pos_topic", "/err_pos");
   std::string err_pos_topic = node_ptr_->get_parameter("tank_robot_plugin.err_pos_topic").as_string();
   m_err_pos_pub = node_ptr_->create_publisher<geometry_msgs::msg::Pose>(err_pos_topic, 10);
+
+  m_tts_pub = node_ptr_->create_publisher<std_msgs::msg::String>("/io/speaker/tts", 1);
+  m_music_pub = node_ptr_->create_publisher<std_msgs::msg::String>("/io/speaker/music", 1);
 }
 
 void TankRobotPlugin::initEstimation()
@@ -394,6 +397,12 @@ void TankRobotPlugin::disabled()
 
 void TankRobotPlugin::autonomous(double current_time)
 {
+ static bool run_yet = 0;
+  if (!run_yet) {
+    playTTS("starting autonomous");
+  run_yet = 1;
+  }
+
   // std::cout << "Autonomous: " << current_time << std::endl;
   bt_->set_variable("auton_time_elapsed", current_time);
 
@@ -434,6 +443,12 @@ void TankRobotPlugin::autonomous(double current_time)
 
 void TankRobotPlugin::teleop(double current_time)
 {
+  static bool run_yet = 0;
+  if (!run_yet) {
+    playMusic("hello_there");
+    run_yet = 1;
+  }
+
   auto joy_data = rhi_ptr_->getMainJoystickData();
   if (joy_data->btn_a && joy_data->btn_b && joy_data->btn_x && joy_data->btn_y &&
     joy_data->btn_u && joy_data->btn_l && joy_data->btn_d && joy_data->btn_r)
@@ -454,7 +469,7 @@ void TankRobotPlugin::teleop(double current_time)
   updateClamp(joy_data);
   updateGoalRush(joy_data);
   updateDrivetrain(joy_data);
-
+  updateMusic(current_time, joy_data);
 }
 
 bool TankRobotPlugin::runAutonFromDriver(std::shared_ptr<JoystickDeviceData> joy_data, double current_time)
@@ -680,6 +695,15 @@ void TankRobotPlugin::updateClamp(std::shared_ptr<JoystickDeviceData> joy_data)
   rhi_ptr_->setDigitalOut(digital_io_port_map["clamp"], m_clamp_closed);
 }
 
+void TankRobotPlugin::updateMusic(double current_time, std::shared_ptr<JoystickDeviceData> joy_data)
+{
+  static double btn_pressed = 0;
+  if (joy_data->btn_u && btn_pressed < (current_time - 5)){
+    btn_pressed = current_time;
+    playMusic(""); // should play random when empty
+  }
+}
+
 void TankRobotPlugin::updateGoalRush(std::shared_ptr<JoystickDeviceData> joy_data)
 {
   static bool goal_rush_btn_pressed = false;
@@ -990,6 +1014,19 @@ void TankRobotPlugin::publishTrajectoryVisualization()
   msg.markers.push_back(marker);
   m_trajectory_viz_pub->publish(msg);
 }
+void TankRobotPlugin::playMusic(std::string musicFileName)
+{
+  auto message = std_msgs::msg::String();
+  message.data = musicFileName;
+  m_music_pub->publish(message);
+}
+
+void TankRobotPlugin::playTTS(std::string textString)
+{
+  auto message = std_msgs::msg::String();
+  message.data = textString;
+  m_tts_pub->publish(message);
+}
 
 } // namespace ghost_tank