Added robot.is_runstopped() and audio for gamepad teleop for homing and runstopped

stretch4_body/behavior/routines/routine_homing.py

@@ -35,7 +35,7 @@ def _unpause_behaviors(self, sm_to_pause, sentry_to_pause):
         self.robot.sentry_manager.unpause(sentry_to_pause)
 
     def run(self, cmd_id, *args, **kwargs):
-        if hasattr(self.robot, 'power_periph') and self.robot.power_periph.status['runstop_event']:
+        if hasattr(self.robot, 'power_periph') and self.robot.is_runstopped():
             self.logger.warning('Not able to home %s. Robot is runstopped' % self.name.capitalize())
             return False
 

stretch4_body/core/gamepad_teleop.py

@@ -10,6 +10,8 @@
 from stretch4_body.core.robot_params import RobotParams
 from stretch4_body.core.feetech.feetech_SM_hello import FeetechCommError
 from stretch4_body.core import gamepad_joints
+from stretch4_body.robot.robot import Robot
+from stretch4_body.robot.robot_client import RobotClient
 from stretch4_body.utils.stretch_pose_models import RobotJoints
 import os
 import time
@@ -295,6 +297,7 @@ def do_motion(self, state = None, robot = None):
                         state['bottom_pad_pressed']
                     )
                     if is_movement_attempt:
+                        play_sound(get_sounds_dir()+f'/homing_required.wav')
                         self.gamepad_controller.vibrate(duration_ms=400, strong_magnitude=1.0, weak_magnitude=1.0)
 
             if self.controller_state is None: # No control if gamepad not being controlled
@@ -303,9 +306,11 @@ def do_motion(self, state = None, robot = None):
             self.manage_start_button(robot)
 
             if robot.is_homed():
-                if self.robot.power_periph.status['runstop_event']:
+                if self.robot.is_runstopped():
+                    play_sound(get_sounds_dir()+f'/is_runstopped.wav')
                     # If the robot is runstopped, vibrate
                     self.gamepad_controller.vibrate(duration_ms=100, strong_magnitude=1.0, weak_magnitude=1.0)
+                    self.logger.error("Robot is runstopped, cannot move.")
                     return False
 
                 # Regular control
@@ -610,7 +615,7 @@ def stop(self):
         self.gamepad_controller.stop()
 
 
-    def manage_start_button(self, robot):
+    def manage_start_button(self, robot:Robot|RobotClient):
         """
         Manage the state of the Start button.
 
@@ -624,6 +629,10 @@ def manage_start_button(self, robot):
 
         if not robot.is_homed():
             def do_home():
+                if self.robot.is_runstopped():
+                    play_sound(get_sounds_dir()+f'/is_runstopped.wav')
+                    self.logger.error("Robot is runstopped, cannot home.")
+                    return
                 self.do_single_beep(robot)
                 play_sound(get_sounds_dir()+f'/homing.wav')
                 robot.home()

stretch4_body/core/hello_utils.py

@@ -810,16 +810,40 @@ def rotation_3x3_matrix(theta):
                      [0            , 0,              1]])
 
 
+
 def get_sounds_dir():
     return str(pathlib.Path(__file__).parent.parent.absolute() / 'media')
 
 
-def play_sound(filename,player='aplay'):
+_sounds_playing = {}
+
+def play_sound(filename, player='aplay', stop_current_playing: bool = True):
     if not os.path.exists(filename):
         print(f"Failed to play sound {filename}: File not found")
         return
+
+    # Clean up sounds that have finished playing
+    for f, proc in list(_sounds_playing.items()):
+        if proc.poll() is not None:
+            del _sounds_playing[f]
+
+    sound_key = os.path.abspath(filename)
+    if (sound_key in _sounds_playing and _sounds_playing[sound_key].poll() is None) or \
+       (filename in _sounds_playing and _sounds_playing[filename].poll() is None):
+        return
+
+    if stop_current_playing:
+        for f, proc in list(_sounds_playing.items()):
+            if proc.poll() is None:
+                try:
+                    proc.terminate()
+                except Exception:
+                    pass
+            del _sounds_playing[f]
+
     try:
-        subprocess.Popen([player, filename], stderr=subprocess.DEVNULL, stdout=subprocess.DEVNULL)
+        proc = subprocess.Popen([player, filename], stderr=subprocess.DEVNULL, stdout=subprocess.DEVNULL)
+        _sounds_playing[sound_key] = proc
     except Exception as e:
         print(f"Failed to play sound {filename}: {e}")
 

stretch4_body/robot/robot_client.py

@@ -149,6 +149,15 @@ def stow(self, do_push=True,wait_on_completion=True,timeout=30, do_pull=True):
         else:
             self.logger.info('Stowing routine started with ID: %d'%rid)
 
+    def trigger_runstop(self):
+        return self.power_periph.trigger_runstop()
+
+    def clear_runstop(self):
+        return self.power_periph.clear_runstop()
+
+    def is_runstopped(self):
+        return self.power_periph.is_runstopped()
+
     def is_homed(self):
         """
         Check if the robot is homed.
@@ -519,6 +528,9 @@ def clear_runstop(self):
     def trigger_runstop(self):
         self._queue_command(subsystem="power_periph", command="trigger_runstop")
 
+    def is_runstopped(self):
+        return self.status['runstop_event']
+
     def set_fan_on(self):
         """
         Turn on the cooling fan.

stretch4_body/robot/robot_core.py

@@ -110,6 +110,15 @@ def is_homed(self):
                 ready = ready and s.is_homed()  
         return ready
 
+    def trigger_runstop(self):
+        return self.power_periph.trigger_runstop()
+
+    def clear_runstop(self):
+        return self.power_periph.clear_runstop()
+
+    def is_runstopped(self):
+        return self.power_periph.is_runstopped()
+
     def wait_command(self, timeout=15.0, use_motion_generator=True):
         raise NotImplementedError('RobotCore::wait_command method not implemented')
 

stretch4_body/subsystem/cameras/calibrate_intrinsics_robot_move.py

@@ -295,7 +295,7 @@ def keyboard_poller():
                 time.sleep(1 / 10)
                 continue
 
-            if self.robot.power_periph.status['runstop_event']:
+            if self.robot.is_runstopped():
                 self.calibration.log_message(
                     f"Runstop event triggered, pausing automatic movement.", LogLevels.INFO
                 )

stretch4_body/subsystem/power_periph.py

@@ -1343,6 +1343,10 @@ def startup(self):
             self.pull_status()
         return self.hw_valid
 
+
+    def is_runstopped(self):
+        return self.status['runstop_event']
+
 if __name__ == '__main__':
     p=PowerPeriph()
     p.startup()

stretch4_body/tools/stretch_keyboard_omni_teleop.py

@@ -54,7 +54,7 @@ def keyboard_control(key:str|None, robot):
     robot.push_command()
 
 def check_runstop(robot):
-    while robot.power_periph.status['runstop_event']:
+    while robot.is_runstopped():
         click.secho("The robot is runstopped", fg="red")
         sleep(1)
 

stretch4_body/tools/stretch_robot_home.py

@@ -20,7 +20,7 @@ def main():
         r = Robot()
         if r.startup():
             r.pull_status()
-            if not r.power_periph.status['runstop_event']:
+            if not r.is_runstopped():
                 r.home()
             else:
                 r.logger.error('Cannot home while run-stopped')

stretch4_body/tools/stretch_robot_stow.py

@@ -19,7 +19,7 @@
 r = Robot()
 if r.startup():
     r.pull_status()
-    if not r.power_periph.status['runstop_event']:
+    if not r.is_runstopped():
         r.stow()
     else:
         r.logger.warning('Cannot stow while run-stopped')

test/test_hello_utils_play_sound.py

@@ -0,0 +1,115 @@
+import os
+import time
+import tempfile
+import unittest
+from unittest.mock import MagicMock, patch
+import subprocess
+
+from stretch4_body.core.hello_utils import play_sound, _sounds_playing, get_sounds_dir
+
+
+class TestPlaySound(unittest.TestCase):
+
+    def setUp(self):
+        _sounds_playing.clear()
+        self.tmp_dir = tempfile.TemporaryDirectory()
+        self.sound1 = os.path.join(self.tmp_dir.name, "sound1.wav")
+        self.sound2 = os.path.join(self.tmp_dir.name, "sound2.wav")
+        with open(self.sound1, "w") as f:
+            f.write("dummy audio content 1")
+        with open(self.sound2, "w") as f:
+            f.write("dummy audio content 2")
+
+    def tearDown(self):
+        _sounds_playing.clear()
+        self.tmp_dir.cleanup()
+
+    @patch("subprocess.Popen")
+    def test_play_sound_file_not_found(self, mock_popen):
+        play_sound("/non/existent/file.wav")
+        mock_popen.assert_not_called()
+        self.assertEqual(len(_sounds_playing), 0)
+
+    @patch("subprocess.Popen")
+    def test_play_sound_new_file(self, mock_popen):
+        mock_proc = MagicMock()
+        mock_proc.poll.return_value = None  # Running
+        mock_popen.return_value = mock_proc
+
+        play_sound(self.sound1)
+
+        mock_popen.assert_called_once_with(["aplay", self.sound1], stderr=subprocess.DEVNULL, stdout=subprocess.DEVNULL)
+        abs_sound1 = os.path.abspath(self.sound1)
+        self.assertIn(abs_sound1, _sounds_playing)
+        self.assertEqual(_sounds_playing[abs_sound1], mock_proc)
+
+    @patch("subprocess.Popen")
+    def test_play_sound_already_playing_does_not_replay(self, mock_popen):
+        mock_proc1 = MagicMock()
+        mock_proc1.poll.return_value = None  # Running
+        mock_popen.return_value = mock_proc1
+
+        play_sound(self.sound1)
+        self.assertEqual(mock_popen.call_count, 1)
+
+        # Try to play sound1 again while it's still running
+        play_sound(self.sound1)
+        self.assertEqual(mock_popen.call_count, 1)  # Should not have called Popen again
+
+    @patch("subprocess.Popen")
+    def test_play_sound_stop_current_playing_true(self, mock_popen):
+        mock_proc1 = MagicMock()
+        mock_proc1.poll.return_value = None  # Running
+        mock_proc2 = MagicMock()
+        mock_proc2.poll.return_value = None  # Running
+
+        mock_popen.side_effect = [mock_proc1, mock_proc2]
+
+        play_sound(self.sound1)
+        self.assertIn(os.path.abspath(self.sound1), _sounds_playing)
+
+        # Play sound2 with stop_current_playing=True (default)
+        play_sound(self.sound2, stop_current_playing=True)
+
+        mock_proc1.terminate.assert_called_once()
+        self.assertNotIn(os.path.abspath(self.sound1), _sounds_playing)
+        self.assertIn(os.path.abspath(self.sound2), _sounds_playing)
+
+    @patch("subprocess.Popen")
+    def test_play_sound_stop_current_playing_false(self, mock_popen):
+        mock_proc1 = MagicMock()
+        mock_proc1.poll.return_value = None  # Running
+        mock_proc2 = MagicMock()
+        mock_proc2.poll.return_value = None  # Running
+
+        mock_popen.side_effect = [mock_proc1, mock_proc2]
+
+        play_sound(self.sound1)
+        play_sound(self.sound2, stop_current_playing=False)
+
+        mock_proc1.terminate.assert_not_called()
+        self.assertIn(os.path.abspath(self.sound1), _sounds_playing)
+        self.assertIn(os.path.abspath(self.sound2), _sounds_playing)
+
+    @patch("subprocess.Popen")
+    def test_cleanup_finished_sounds(self, mock_popen):
+        mock_proc1 = MagicMock()
+        mock_proc1.poll.return_value = 0  # Finished
+        mock_popen.return_value = mock_proc1
+
+        abs_sound1 = os.path.abspath(self.sound1)
+        _sounds_playing[abs_sound1] = mock_proc1
+
+        mock_proc2 = MagicMock()
+        mock_proc2.poll.return_value = None  # Running
+        mock_popen.return_value = mock_proc2
+
+        # Calling play_sound on sound2 should clean up sound1 because sound1 is finished
+        play_sound(self.sound2)
+
+        self.assertNotIn(abs_sound1, _sounds_playing)
+        self.assertIn(os.path.abspath(self.sound2), _sounds_playing)
+
+
+if __name__ == "__main__":
+    unittest.main()