audible feedback

autonav_ws/src/autonav_hardware/CMakeLists.txt

@@ -0,0 +1,64 @@
+cmake_minimum_required(VERSION 3.8)
+
+project(autonav_hardware)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclpy REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+find_package(autonav_shared REQUIRED)
+find_package(autonav_msgs REQUIRED)
+# uncomment the following section in order to fill in
+# further dependencies manually.
+# find_package(<dependency> REQUIRED)
+
+# C++
+
+# Inlcude Cpp "include" directory
+include_directories(include)
+
+# Create Cpp executables
+#add_executable(executable_name path_to_executable/executable.cpp)
+#ament_target_dependencies(executable_name rclcpp other_dependencies)
+
+# Install Cpp executables
+install(TARGETS
+  # install executables by name
+  # executable_name
+  DESTINATION lib/${PROJECT_NAME}
+ )
+
+# Python
+
+ # Use only if not using rosidl_generate_interfaces
+ # Install Python modules
+ #ament_python_install_package(${PROJECT_NAME})
+
+ # Install Python programs
+ install(PROGRAMS
+   # add programs in format:
+   #/path_to_program/program.py
+   src/audible_feedback.py
+   src/simple_audible_feedback_publisher.py
+   DESTINATION lib/${PROJECT_NAME}
+)
+
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # comment the line when a copyright and license is added to all source files
+  set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # comment the line when this package is in a git repo and when
+  # a copyright and license is added to all source files
+  set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()

autonav_ws/src/autonav_hardware/LICENSE

@@ -0,0 +1,17 @@
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in
+all copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
+THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+THE SOFTWARE.

autonav_ws/src/autonav_hardware/package.xml

@@ -0,0 +1,25 @@
+<?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>autonav_hardware</name>
+  <version>2025.0.1</version>
+  <description>Contains ROS nodes responsible for interacting with hardware devices</description>
+  <maintainer email="[email protected]">tony</maintainer>
+  <license>MIT</license>
+
+  <build_depend>rosidl_default_generators</build_depend>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <buildtool_depend>ament_cmake_python</buildtool_depend>
+
+  <depend>rclpy</depend>
+  <depend>autonav_shared</depend>
+  <depend>autonav_msgs</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

autonav_ws/src/autonav_hardware/src/audible_feedback.py

@@ -0,0 +1,155 @@
+#!/usr/bin/env python3
+
+
+import rclpy
+from autonav_msgs.msg import AudibleFeedback
+
+from autonav_shared.node import Node
+from autonav_shared.types import LogLevel, DeviceState, SystemState
+import time
+import os
+from just_playback import Playback
+import PySoundSphere
+
+
+class AudibleFeedbackConfig:
+    def __init__(self):
+        self.volume = 1.0
+        self.autonomous_transition_filepath = os.path.expanduser('~/Documents/imposter.mp3')
+
+
+class AudibleFeedbackNode(Node):
+    def __init__(self):
+        super().__init__("audible_feedback_node")
+        self.write_config(AudibleFeedbackConfig())
+        self.current_playing_thread = None
+        self.secondary_tracks = []
+        self.main_track = None
+        self.old_system_state = self.system_state
+
+        # 
+        self.audible_feedback_subscriber = self.create_subscription(
+            AudibleFeedback, 
+            "/autonav/audible_feedback",
+            self.on_audible_feedback_received,
+            20
+        )
+
+        self.system_state_monitor = self.create_timer(0.5, self.monitor_system_state)
+        self.track_monitor = self.create_timer(0.5, self.monitor_tracks)
+
+    def on_audible_feedback_received(self, msg:AudibleFeedback):
+        self.monitor_tracks()
+
+        # self.log(f"{len(self.secondary_tracks)}", LogLevel.DEBUG)
+        if msg.stop_all:
+            self.stop_all()
+            return
+
+        if msg.pause_all:
+            self.pause_main_track()
+            return
+
+        if msg.unpause_all:
+            self.unpause_main_track()
+            return
+
+        else:
+            # self.log(f"playing {msg.filename}")
+            filename = str(msg.filename)
+            main_track = msg.main_track
+
+            # self.log(f"heard request to play {msg.filename}")
+            self.play_sound(filename, main_track)
+
+
+    def play_sound(self, filename, main_track: bool):
+        if main_track and self.main_track is not None:
+            return
+
+        playback = PySoundSphere.AudioPlayer("ffplay", debug_allow_multiple_playbacks=False)
+        try:
+            playback.load(filename)
+        except:
+            self.log("invalid filename", LogLevel.ERROR)
+            return
+
+        playback.volume = self.config.get('volume')
+        playback.play()
+
+        if main_track:
+            self.main_track = playback
+
+        else:
+            self.secondary_tracks.append(playback)
+
+
+    def stop_all(self):
+        if self.main_track is not None:
+            self.log(f"{self.main_track}")
+
+        for track in self.secondary_tracks:
+            self.log(f"{track}")
+
+        for track in self.secondary_tracks:
+            track.stop()
+
+        self.secondary_tracks = []
+        try:
+            self.main_track.stop()
+        except:
+            self.log("No main track", LogLevel.ERROR)
+
+        self.main_track = None
+
+
+    def pause_main_track(self):
+        try:
+            self.main_track.pause()
+        except: # main track is paused or doesn't exist
+            pass
+
+
+    def unpause_main_track(self):
+            try:
+                self.main_track.play()
+
+            except: # main track is playing or doesn't exist
+                pass
+
+
+    def monitor_tracks(self):
+        if len(self.secondary_tracks) > 16:
+            self.secondary_tracks.pop()
+
+
+    def monitor_system_state(self):
+        self.monitor_tracks()
+        if self.system_state == SystemState.AUTONOMOUS and self.old_system_state != SystemState.AUTONOMOUS:
+            playback = PySoundSphere.AudioPlayer("ffplay", debug_allow_multiple_playbacks = False)
+
+            try:
+                filename = self.config.get('autonomous_transition_filepath')
+            except:
+                self.log("invalid autonomous transition filepath")
+                return
+
+            playback.load(filename)
+            playback.volume = self.config.get('volume')
+            playback.play()
+
+            self.secondary_tracks.append(playback)
+            self.old_system_state = SystemState.AUTONOMOUS
+
+        else:
+            self.old_system_state = self.system_state
+
+
+def main():
+    rclpy.init()
+    audible_feedback_node = AudibleFeedbackNode()
+    rclpy.spin(audible_feedback_node)
+    rclpy.shutdown()
+
+if __name__ == "__main__":
+    main()

autonav_ws/src/autonav_hardware/src/simple_audible_feedback_publisher.py

@@ -0,0 +1,56 @@
+#!/usr/bin/env python3
+
+import rclpy
+from rclpy.node import Node
+
+from autonav_msgs.msg import AudibleFeedback
+import os
+
+
+class MinimalAudibleFeedbackPublisher(Node):
+
+    def __init__(self):
+        super().__init__('minimal_publisher')
+        self.publisher_ = self.create_publisher(
+            AudibleFeedback,
+            '/autonav/audible_feedback',
+            20
+            )
+
+        timer_period = 3 # seconds
+        self.timer = self.create_timer(timer_period, self.timer_callback)
+        self.i = 0
+
+
+    def timer_callback(self):
+        msg = AudibleFeedback()
+        if self.i == 0:
+            msg.filename = os.path.expanduser("~/Documents/vivalavida.wav")
+
+        elif self.i % 3 == 0:
+            msg.stop_all = True
+
+        else:
+            msg.filename = os.path.expanduser("~/Documents/metal-pipe.wav")
+
+        self.publisher_.publish(msg)
+        self.get_logger().info('Publishing: "%s"' % msg.filename)
+        self.i += 1
+
+
+def main(args=None):
+    rclpy.init(args=args)
+
+    minimal_publisher = MinimalAudibleFeedbackPublisher()
+
+    rclpy.spin(minimal_publisher)
+
+    # Destroy the node explicitly
+    # (optional - otherwise it will be done automatically
+    # when the garbage collector destroys the node object)
+    minimal_publisher.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

autonav_ws/src/autonav_hardware/src/soundsphere.py

@@ -0,0 +1,13 @@
+import PySoundSphere
+import os
+import time
+
+player = PySoundSphere.AudioPlayer("ffplay")
+player.load(os.path.expanduser("~/Documents/vivalavida.wav"))
+
+player.volume = 1.0
+
+print(player)
+player.play()
+
+time.sleep(10)

autonav_ws/src/autonav_launch/launch/manual24sim.xml

@@ -1,5 +1,5 @@
 <launch>
-  <include file="$(find-pkg-share rosbridge_server)/launch/rosbridge_websocket_launch.xml" />
+  <node pkg="ros_tcp_endpoint" exec="default_server_endpoint" output="screen" emulate_tty="true" />
 
   <!-- xbox manual control -->
   <node pkg="autonav_manual" exec="controller_input.py"/>

autonav_ws/src/autonav_launch/launch/manual25.xml

@@ -1,7 +1,8 @@
 <launch>
   <!-- Manual controller -->
-  <node pkg="autonav_manual" exec="controller_input.py"/>
-  <node pkg="autonav_manual" exec="manual_25.py"/>
+  <node pkg="autonav_manual" exec="controller_input.py" output="screen" emulate_tty="true" />
+  <node pkg="autonav_manual" exec="manual_25.py" output="screen" emulate_tty="true" />
+  <node pkg="autonav_hardware" exec="audible_feedback.py" output="screen" emulate_tty="true" />
   <!-- <node pkg="autonav_manual" exec="motormessage_listener.py"/> -->
 
   <!-- Other -->

autonav_ws/src/autonav_launch/launch/manual25sim.xml

@@ -1,9 +1,11 @@
 <launch>
-  <include file="$(find-pkg-share rosbridge_server)/launch/rosbridge_websocket_launch.xml" />
+  <node pkg="ros_tcp_endpoint" exec="default_server_endpoint" output="screen" emulate_tty="true" />
+
 
   <!-- xbox manual control -->
   <node pkg="autonav_manual" exec="controller_input.py"/>
   <node pkg="autonav_manual" exec="manual_25.py"/>
+  <node pkg="autonav_hardware" exec="audible_feedback.py" output="screen" emulate_tty="true" />
   <!-- <node pkg="autonav_manual" exec="motormessage_listener.py"/> -->
 
   <!-- Other -->

autonav_ws/src/autonav_launch/launch/test.xml

@@ -1,4 +1,5 @@
 <launch>
     <node pkg="autonav_example_py" exec="example.py" output="screen" emulate_tty="true" />
     <node pkg="autonav_example_cpp" exec="autonav_example_cpp" output="screen" emulate_tty="true" />
+    <node pkg="ros_tcp_endpoint" exec="default_server_endpoint" output="screen" emulate_tty="true" />
 </launch>

autonav_ws/src/autonav_manual/src/manual_24.py

@@ -38,7 +38,7 @@ def init(self):
 
         self.motorPublisher = self.create_publisher(
             MotorInput,
-            '/autonav/MotorInput',
+            '/autonav/motor_input',
             10
         )
 
@@ -82,6 +82,11 @@ def change_system_state(self):
             self.log(f'Setting system state to {new_system_state}')
             self.set_system_state(new_system_state)
 
+        elif self.controller_state['btn_mode'] == 1.0:
+            new_system_state = SystemState.AUTONOMOUS
+            self.log(f'Setting system state to {new_system_state}')
+            self.set_system_state(new_system_state)
+
         elif self.controller_state['btn_select'] == 1.0:
             new_system_state = SystemState.DISABLED
             self.log(f'Setting system state to {new_system_state}')