From 3f8b702085fd9c8f8e50c6530b45f930ad8f1624 Mon Sep 17 00:00:00 2001
From: silencht <silencht@qq.com>
Date: Tue, 4 Nov 2025 09:07:18 +0800
Subject: [PATCH] [fix] bugs and bump televuer version

---
 teleop/teleop_hand_and_arm.py   | 72 ++++++++++++++++++++++-----------
 teleop/televuer                 |  2 +-
 teleop/utils/motion_switcher.py |  1 +
 3 files changed, 50 insertions(+), 25 deletions(-)

diff --git a/teleop/teleop_hand_and_arm.py b/teleop/teleop_hand_and_arm.py
index 91c94da..77b1608 100644
--- a/teleop/teleop_hand_and_arm.py
+++ b/teleop/teleop_hand_and_arm.py
@@ -123,6 +123,15 @@ if __name__ == '__main__':
                                      img_shape=camera_config['head_camera']['image_shape'],
                                      webrtc=camera_config['head_camera']['enable_webrtc'],
                                      webrtc_url=f"https://{args.img_server_ip}:{camera_config['head_camera']['webrtc_port']}/offer")
+        
+        # motion mode (G1: Regular mode R1+X, not Running mode R2+A)
+        if args.motion:
+            if args.xr_mode == "controller":
+                loco_wrapper = LocoClientWrapper()
+        else:
+            motion_switcher = MotionSwitcher()
+            status, result = motion_switcher.Enter_Debug_Mode()
+            logger_mp.info(f"Enter debug mode: {'Success' if status == 0 else 'Failed'}")
 
         # arm
         if args.arm == "G1_29":
@@ -206,15 +215,6 @@ if __name__ == '__main__':
             from teleop.utils.sim_state_topic import start_sim_state_subscribe
             sim_state_subscriber = start_sim_state_subscribe()
 
-        # motion mode (G1: Regular mode R1+X, not Running mode R2+A)
-        if args.motion:
-            if args.xr_mode == "controller":
-                loco_wrapper = LocoClientWrapper()
-        else:
-            motion_switcher = MotionSwitcher()
-            status, result = motion_switcher.Enter_Debug_Mode()
-            logger_mp.info(f"Enter debug mode: {status}, {result}")
-        
         # record + headless / non-headless mode
         if args.record:
             recorder = EpisodeWriter(task_dir = os.path.join(args.task_dir, args.task_name),
@@ -460,23 +460,47 @@ if __name__ == '__main__':
     except KeyboardInterrupt:
         logger_mp.info("KeyboardInterrupt, exiting program...")
     finally:
-        arm_ctrl.ctrl_dual_arm_go_home()
-        img_client.close()
-        tv_wrapper.close()
-        if not args.motion:
-            status, result = motion_switcher.Exit_Debug_Mode()
-            logger_mp.info(f"Exit debug mode: {status}, {result}")
+        try:
+            arm_ctrl.ctrl_dual_arm_go_home()
+        except Exception as e:
+            logger_mp.error(f"Failed to ctrl_dual_arm_go_home: {e}")
+        
+        try:
+            if args.ipc:
+                ipc_server.stop()
+            else:
+                stop_listening()
+                listen_keyboard_thread.join()
+        except Exception as e:
+            logger_mp.error(f"Failed to stop keyboard listener or ipc server: {e}")
+        
+        try:
+            img_client.close()
+        except Exception as e:
+            logger_mp.error(f"Failed to close image client: {e}")
 
-        if args.ipc:
-            ipc_server.stop()
-        else:
-            stop_listening()
-            listen_keyboard_thread.join()
+        try:
+            tv_wrapper.close()
+        except Exception as e:
+            logger_mp.error(f"Failed to close televuer wrapper: {e}")
 
-        if args.sim:
-            sim_state_subscriber.stop_subscribe()
+        try:
+            if not args.motion:
+                status, result = motion_switcher.Exit_Debug_Mode()
+                logger_mp.info(f"Exit debug mode: {'Success' if status == 3104 else 'Failed'}")
+        except Exception as e:
+            logger_mp.error(f"Failed to exit debug mode: {e}")
 
-        if args.record:
-            recorder.close()
+        try:
+            if args.sim:
+                sim_state_subscriber.stop_subscribe()
+        except Exception as e:
+            logger_mp.error(f"Failed to stop sim state subscriber: {e}")
+        
+        try:
+            if args.record:
+                recorder.close()
+        except Exception as e:
+            logger_mp.error(f"Failed to close recorder: {e}")
         logger_mp.info("Finally, exiting program.")
         exit(0)
\ No newline at end of file
diff --git a/teleop/televuer b/teleop/televuer
index 740b2bd..d3cdb6e 160000
--- a/teleop/televuer
+++ b/teleop/televuer
@@ -1 +1 @@
-Subproject commit 740b2bdb173b7cab52e9a888ce73976f59be7a59
+Subproject commit d3cdb6efa290506ab7e16f15fe3e6180c9245c20
diff --git a/teleop/utils/motion_switcher.py b/teleop/utils/motion_switcher.py
index fc451be..3a4d6bf 100644
--- a/teleop/utils/motion_switcher.py
+++ b/teleop/utils/motion_switcher.py
@@ -8,6 +8,7 @@ import time
 # MotionSwitcher used to switch mode between debug mode and ai mode
 class MotionSwitcher:
     def __init__(self):
+        ChannelFactoryInitialize(0)
         self.msc = MotionSwitcherClient()
         self.msc.SetTimeout(1.0)
         self.msc.Init()