[fix] motion switcher.

teleop/teleop_hand_and_arm.py

@@ -212,8 +212,8 @@ if __name__ == '__main__':
                 loco_wrapper = LocoClientWrapper()
         else:
             motion_switcher = MotionSwitcher()
-            # status, result = motion_switcher.Enter_Debug_Mode()
-            # logger_mp.info(f"Enter debug mode: {status}, {result}")
+            status, result = motion_switcher.Enter_Debug_Mode()
+            logger_mp.info(f"Enter debug mode: {status}, {result}")
         
         # record + headless / non-headless mode
         if args.record:

teleop/utils/motion_switcher.py

@@ -13,16 +13,22 @@ class MotionSwitcher:
         self.msc.Init()
 
     def Enter_Debug_Mode(self):
+        try:
             status, result = self.msc.CheckMode()
             while result['name']:
                 self.msc.ReleaseMode()
                 status, result = self.msc.CheckMode()
                 time.sleep(1)
             return status, result
+        except Exception as e:
+            return None, None
     
     def Exit_Debug_Mode(self):
+        try:
             status, result = self.msc.SelectMode(nameOrAlias='ai')
             return status, result
+        except Exception as e:
+            return None, None
 
 class LocoClientWrapper:
     def __init__(self):