added some sounds, just_playback can be overloaded though

autonav_ws/src/autonav_hardware/CMakeLists.txt

@@ -43,7 +43,7 @@ install(TARGETS
  install(PROGRAMS
    # add programs in format:
    #/path_to_program/program.py
-   src/audible_feedback_node.py
+   src/audible_feedback.py
    src/simple_audible_feedback_publisher.py
    DESTINATION lib/${PROJECT_NAME}
 )

autonav_ws/src/autonav_hardware/src/audible_feedback_node.py → autonav_ws/src/autonav_hardware/src/audible_feedback.py

@@ -7,8 +7,6 @@
 from autonav_shared.node import Node
 from autonav_shared.types import LogLevel, DeviceState, SystemState
 import time
-import threading
-import just_playback
 import os
 from just_playback import Playback
 
@@ -25,17 +23,23 @@ def __init__(self):
         self.write_config(AudibleFeedbackConfig())
         self.current_playing_thread = None
         self.tracks = []
+        self.old_system_state = self.system_state
 
         # 
         self.audible_feedback_subscriber = self.create_subscription(
             AudibleFeedback, 
-            "autonav/audible_feedback",
+            "/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.tracks)}", LogLevel.ERROR)
         if msg.stop_all:
             self.stop_all()
 
@@ -63,6 +67,27 @@ def stop_all(self):
             track.stop()
 
 
+    def monitor_tracks(self):
+        for track in self.tracks:
+            self.log(f"{track.playing}")
+            if track.playing == False:
+                track.stop()
+                self.tracks.remove(track)
+
+
+    def monitor_system_state(self):
+        self.monitor_tracks()
+        if self.system_state == SystemState.AUTONOMOUS and self.old_system_state != SystemState.AUTONOMOUS:
+            playback = Playback()
+            filename = os.path.expanduser('~/Documents/imposter.mp3')
+            playback.load_file(filename)
+            playback.play()
+            self.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()

autonav_ws/src/autonav_hardware/src/simple_audible_feedback_publisher.py

@@ -13,7 +13,7 @@ def __init__(self):
         super().__init__('minimal_publisher')
         self.publisher_ = self.create_publisher(
             AudibleFeedback,
-            'autonav/audible_feedback',
+            '/autonav/audible_feedback',
             20
             )
 

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_manual/src/manual_25.py

@@ -1,15 +1,15 @@
 #!/usr/bin/env python3
 
 import rclpy
-import json
 from autonav_msgs.msg import ControllerInput, MotorInput, MotorFeedback
-from types import SimpleNamespace
 import numpy as np
 from autonav_shared.node import Node
 from autonav_shared.types import LogLevel, DeviceState, SystemState
-from autonav_msgs.msg import ControllerInput
+from autonav_msgs.msg import AudibleFeedback, ControllerInput
 from enum import IntEnum
 import time
+import json
+import os
 
 class ControllerMode(IntEnum):
     LOCAL = 0
@@ -22,12 +22,14 @@ def __init__(self):
         self.max_sideways_speed = 3
         self.max_angular_speed = np.pi
         self.odom_fudge_factor = 1
+        self.sound_buffer = 0.1 # seconds
 
 
 class Manual25Node(Node):
     def __init__(self):
         super().__init__('manual25_node')
         self.write_config(Manual25Config())
+        self.log("bigger pepe")
 
 
     def init(self):
@@ -37,11 +39,7 @@ def init(self):
         self.delta_t = 0
         self.new_time = time.time()
         # self.max_forward_speed = 1
-        self.max_forward_speed = self.config.max_forward_speed
-        self.max_sideways_speed = self.config.max_sideways_speed
         # self.max_angular_speed = np.pi/4
-        self.max_angular_speed = self.config.max_angular_speed
-        self.odom_fudge_factor = self.config.odom_fudge_factor
 
         self.controller_state = {}
 
@@ -67,17 +65,31 @@ def init(self):
             10
         )
 
+        self.audibleFeedbackPublisher = self.create_publisher(
+            AudibleFeedback,
+            '/autonav/audible_feedback',
+            10
+        )
+
         self.controllerSubscriber  # prevent unused variable warning
 
 
     def input_callback(self, msg):
+        self.new_time = time.time()
+        self.delta_t = self.new_time - self.last_time
+
         self.set_device_state(DeviceState.OPERATING)
         self.deserialize_controller_state(msg)
         # self.log(f"I heard: {str(self.controller_state)}")
 
         self.change_controller_mode()
         self.change_system_state()
 
+        if self.delta_t > self.config.get("sound_buffer"):
+            self.play_sound()
+            self.last_time = time.time()
+
+
         self.log(f"orientation: {self.orientation}")
         # local vs. global toggle
 
@@ -87,7 +99,6 @@ def input_callback(self, msg):
             self.compose_motorinput_message_global()
 
 
-
     def deserialize_controller_state(self, msg):
         attributes = [n for n in dir(msg) if not (n.startswith('__') or n.startswith('_'))]
         attributes.remove('SLOT_TYPES')
@@ -139,9 +150,9 @@ def compose_motorinput_message_local(self):
         sideways_velocity = 0.0
         angular_velocity = 0.0
         if self.system_state == SystemState.MANUAL:
-            forward_velocity = self.normalize(self.controller_state["abs_y"], -self.max_forward_speed, self.max_forward_speed, 1.0, -1.0)
-            sideways_velocity = self.normalize(self.controller_state["abs_x"], -self.max_sideways_speed, self.max_sideways_speed, -1.0, 1.0)
-            angular_velocity = self.normalize(self.controller_state["abs_z"], -self.max_angular_speed, self.max_angular_speed, 1.0, -1.0)
+            forward_velocity = self.normalize(self.controller_state["abs_y"], -self.config.get("max_forward_speed"), self.config.get("max_forward_speed"), 1.0, -1.0)
+            sideways_velocity = self.normalize(self.controller_state["abs_x"], -self.config.get("max_sideways_speed"), self.config.get("max_sideways_speed"), -1.0, 1.0)
+            angular_velocity = self.normalize(self.controller_state["abs_z"], -self.config.get("max_angular_speed"), self.config.get("max_angular_speed"), 1.0, -1.0)
 
         motor_msg = MotorInput()
         motor_msg.forward_velocity = forward_velocity
@@ -150,15 +161,16 @@ def compose_motorinput_message_local(self):
 
         self.motorPublisher.publish(motor_msg)
 
+
     # https://math.stackexchange.com/questions/2895880/inversion-of-rotation-matrix
     def compose_motorinput_message_global(self):
         forward_velocity = 0.0
         sideways_velocity = 0.0
         angular_velocity = 0.0
         if self.system_state == SystemState.MANUAL:
-            forward_velocity = self.normalize(self.controller_state["abs_y"], -self.max_forward_speed, self.max_forward_speed, 1.0, -1.0)
-            sideways_velocity = self.normalize(self.controller_state["abs_x"], -self.max_sideways_speed, self.max_sideways_speed, -1.0, 1.0)
-            angular_velocity = self.normalize(self.controller_state["abs_z"], -self.max_angular_speed, self.max_angular_speed, 1.0, -1.0)
+            forward_velocity = self.normalize(self.controller_state["abs_y"], -self.config.get("max_forward_speed"), self.config.get("max_forward_speed"), 1.0, -1.0)
+            sideways_velocity = self.normalize(self.controller_state["abs_x"], -self.config.get("max_sideways_speed"), self.config.get("max_sideways_speed"), -1.0, 1.0)
+            angular_velocity = self.normalize(self.controller_state["abs_z"], -self.config.get("max_angular_speed"), self.config.get("max_angular_speed"), 1.0, -1.0)
 
         motor_msg = MotorInput()
         motor_msg.forward_velocity = forward_velocity * np.cos(self.orientation) + sideways_velocity * np.sin(self.orientation)
@@ -169,9 +181,18 @@ def compose_motorinput_message_global(self):
 
 
     def on_motor_feedback(self, msg:MotorFeedback):
-        delta_theta = msg.delta_theta * self.odom_fudge_factor
+        delta_theta = msg.delta_theta * self.config.get("odom_fudge_factor")
         self.orientation += delta_theta
 
+
+    def play_sound(self):
+        if self.controller_state['btn_west'] == 1:
+            audible_feedback = AudibleFeedback()
+            audible_feedback.filename = os.path.expanduser('~/Documents/vine-boom.mp3')
+            audible_feedback.stop_all = False
+            self.audibleFeedbackPublisher.publish(audible_feedback)
+
+
 def main(args=None):
     rclpy.init()
     node = Manual25Node()