[fix] bug.

README.md

@@ -189,8 +189,8 @@ This program supports XR control of a physical robot or in simulation. Choose mo
 
 |    ⚙️ Flag    |                        📜 Description                         |
 | :----------: | :----------------------------------------------------------: |
-|  `--record`  | **Enable data recording**: after pressing **r** to start, press **s** to start/stop saving an episode. Can repeat. |
-|  `--motion`  | **Enable motion control**: allows independent robot control during teleop. In hand mode, use the R3 remote for walking; in controller mode, use joystick for walking |
+|  `--record`  | Enable **data recording**<br />After pressing **r** to start, press **s** to start/stop saving an episode. Can repeat. |
+|  `--motion`  | Enable **motion mode**<br />After enabling this mode, the teleoperation program can run alongside the robot's motion control.<br />In **hand tracking** mode, you can use the [R3 Controller](https://www.unitree.com/cn/R3) to control the robot's walking behavior; <br />in **controller tracking** mode, you can also use [controllers to control the robot’s movement](https://github.com/unitreerobotics/xr_teleoperate/blob/375cdc27605de377c698e2b89cad0e5885724ca6/teleop/teleop_hand_and_arm.py#L247-L257). |
 | `--headless` |        Run without GUI (for headless PC2 deployment)         |
 |   `--sim`    |                  Enable **simulation mode**                  |
 
@@ -318,7 +318,7 @@ If two hands open and close continuously, it indicates success. Once successful,
 
 ## 3.3 🚀 Launch
 
-> ![Warning](https://img.shields.io/badge/Warning-Important-red) 
+>  ![Warning](https://img.shields.io/badge/Warning-Important-red)
 >
 > 1. Everyone must keep a safe distance from the robot to prevent any potential danger!
 > 2. Please make sure to read the [Official Documentation](https://support.unitree.com/home/zh/Teleoperation) at least once before running this program.
@@ -336,6 +336,7 @@ Same as simulation but follow the safety warnings above.
 ## 3.4 🔚 Exit
 
 > ![Warning](https://img.shields.io/badge/Warning-Important-red)
+>
 > To avoid damaging the robot, it is recommended to position the robot's arms close to the initial pose before pressing **q** to exit.
 >
 > - In **Debug Mode**: After pressing the exit key, both arms will return to the robot's **initial pose** within 5 seconds, and then the control will end.

README_ja-JP.md

@@ -183,7 +183,7 @@ G1(29 DoF)とDex3ハンド構成でシミュレーションを起動:
 |   ⚙️ フラグ   |                            📜 説明                            |
 | :----------: | :----------------------------------------------------------: |
 |  `--record`  | **データ記録有効化**: **r**押下で開始後、**s**でエピソード記録開始/停止。繰り返し可能 |
-|  `--motion`  | **モーション制御有効化**: 遠隔操作中に独立したロボット制御を許可。ハンドモードではR3リモコンで歩行、コントローラーモードではジョイスティックで歩行 |
+|  `--motion`  | **モーション制御有効化**: 遠隔操作中に独立したロボット制御を許可。<br />ハンドモードではR3リモコンで歩行、コントローラーモードではジョイスティックで歩行 |
 | `--headless` |             GUIなしで実行（ヘッドレスPC2展開用）             |
 |   `--sim`    |               **シミュレーションモード**有効化               |
 

teleop/teleop_hand_and_arm.py

@@ -404,15 +404,15 @@ if __name__ == '__main__':
         logger_mp.info("KeyboardInterrupt, exiting program...")
     finally:
         arm_ctrl.ctrl_dual_arm_go_home()
-        tv_img_shm.unlink()
+        if args.sim:
+            sim_state_subscriber.stop_subscribe()
         tv_img_shm.close()
+        tv_img_shm.unlink()
         if WRIST:
-            wrist_img_shm.unlink()
             wrist_img_shm.close()
+            wrist_img_shm.unlink()
         if args.record:
             recorder.close()
-        if args.sim:
-            sim_state_subscriber.stop_subscribe()
         listen_keyboard_thread.join()
         logger_mp.info("Finally, exiting program...")
         exit(0)

teleop/utils/episode_writer.py

@@ -114,7 +114,7 @@ class EpisodeWriter():
         logger_mp.info(f"==> New episode created: {self.episode_dir}")
         return True  # Return True if the episode is successfully created
         
-    def add_item(self, colors, depths=None, states=None, actions=None, tactiles=None, audios=None,sim_state=None):
+    def add_item(self, colors, depths=None, states=None, actions=None, tactiles=None, audios=None, sim_state=None):
         # Increment the item ID
         self.item_id += 1
         # Create the item data dictionary

teleop/utils/sim_state_topic.py

@@ -20,7 +20,7 @@ logger_mp = logging_mp.get_logger(__name__)
 class SharedMemoryManager:
     """Shared memory manager"""
     
-    def __init__(self, name: str = None, size: int = 512):
+    def __init__(self, name: Optional[str] = None, size: int = 512):
         """Initialize shared memory manager
         
         Args:
@@ -118,7 +118,6 @@ class SharedMemoryManager:
         """Destructor"""
         self.cleanup()
 
-
 class SimStateSubscriber:
     """Simple sim state subscriber class"""
     
@@ -139,80 +138,69 @@ class SimStateSubscriber:
         # initialize shared memory
         self._setup_shared_memory()
         
-        logger_mp.info(f"[SimStateSubscriber] Initialized with shared memory: {shm_name}")
+        logger_mp.debug(f"[SimStateSubscriber] Initialized with shared memory: {shm_name}")
     
     def _setup_shared_memory(self):
         """Setup shared memory"""
         try:
             self.shared_memory = SharedMemoryManager(self.shm_name, self.shm_size)
-            logger_mp.info(f"[SimStateSubscriber] Shared memory setup successfully")
+            logger_mp.debug(f"[SimStateSubscriber] Shared memory setup successfully")
         except Exception as e:
             logger_mp.error(f"[SimStateSubscriber] Failed to setup shared memory: {e}")
     
-    def _message_handler(self, msg: String_):
-        """Handle received messages"""
-        try:
-            # parse the received message
-            data = json.loads(msg.data)
-            
-            # write to shared memory
-            if self.shared_memory:
-                self.shared_memory.write_data(data)
-                
-        except Exception as e:
-            logger_mp.error(f"[SimStateSubscriber] Error processing message: {e}")
-    
-    def _subscribe_loop(self):
-        """Subscribe loop thread"""
-        logger_mp.info(f"[SimStateSubscriber] Subscribe thread started")
-        
-        while self.running:
-            try:
-                time.sleep(0.001)  # keep thread alive
-            except Exception as e:
-                logger_mp.error(f"[SimStateSubscriber] Error in subscribe loop: {e}")
-                time.sleep(0.01)
-        
-        logger_mp.info(f"[SimStateSubscriber] Subscribe thread stopped")
-    
     def start_subscribe(self):
         """Start subscribing"""
         if self.running:
-            logger_mp.info(f"[SimStateSubscriber] Already running")
+            logger_mp.warning(f"[SimStateSubscriber] Already running")
             return
         
         try:
-            # setup subscriber
             self.subscriber = ChannelSubscriber("rt/sim_state", String_)
-            self.subscriber.Init(self._message_handler, 10)
-            
+            self.subscriber.Init()
             self.running = True
-            logger_mp.info(f"[SimStateSubscriber] Subscriber initialized")
-            # start subscribe thread
-            self.subscribe_thread = threading.Thread(target=self._subscribe_loop)
-            self.subscribe_thread.daemon = True
+
+            self.subscribe_thread = threading.Thread(target=self._subscribe_sim_state, daemon=True)
             self.subscribe_thread.start()
-            
-            logger_mp.info(f"[SimStateSubscriber] Started subscribing to rt/sim_state_cmd")
+
+            logger_mp.info(f"[SimStateSubscriber] Started subscribing to rt/sim_state")
             
         except Exception as e:
             logger_mp.error(f"[SimStateSubscriber] Failed to start subscribing: {e}")
             self.running = False
-    
+
+    def _subscribe_sim_state(self):
+        """Subscribe loop thread"""
+        logger_mp.debug(f"[SimStateSubscriber] Subscribe thread started")
+        
+        while self.running:
+            try:
+                if self.subscriber:
+                    msg = self.subscriber.Read()
+                    if msg:
+                        data = json.loads(msg.data)
+                    else:
+                        logger_mp.warning("[SimStateSubscriber] Received None message")
+                    if self.shared_memory and data:
+                        self.shared_memory.write_data(data)
+                else:
+                    logger_mp.error("[SimStateSubscriber] Subscriber is not initialized")
+                time.sleep(0.002)
+            except Exception as e:
+                logger_mp.error(f"[SimStateSubscriber] Error in subscribe loop: {e}")
+                time.sleep(0.01)
+
     def stop_subscribe(self):
         """Stop subscribing"""
         if not self.running:
+            logger_mp.warning(f"[SimStateSubscriber] Already stopped or not running")
             return
 
-        logger_mp.info(f"[SimStateSubscriber] Stopping subscriber...")
         self.running = False
-        
         # wait for thread to finish
         if self.subscribe_thread:
             self.subscribe_thread.join(timeout=1.0)
 
         if self.shared_memory:
-            # cleanup shared memory
             self.shared_memory.cleanup()
         logger_mp.info(f"[SimStateSubscriber] Subscriber stopped")
     
@@ -239,20 +227,6 @@ class SimStateSubscriber:
         self.stop_subscribe()
 
 
-# convenient functions
-def create_sim_state_subscriber(shm_name: str = "sim_state_cmd_data", shm_size: int = 3096) -> SimStateSubscriber:
-    """Create a sim state subscriber instance
-    
-    Args:
-        shm_name: shared memory name
-        shm_size: shared memory size
-        
-    Returns:
-        SimStateSubscriber: subscriber instance
-    """
-    return SimStateSubscriber(shm_name, shm_size)
-
-
 def start_sim_state_subscribe(shm_name: str = "sim_state_cmd_data", shm_size: int = 3096) -> SimStateSubscriber:
     """Start sim state subscribing
     
@@ -263,7 +237,7 @@ def start_sim_state_subscribe(shm_name: str = "sim_state_cmd_data", shm_size: in
     Returns:
         SimStateSubscriber: started subscriber instance
     """
-    subscriber = create_sim_state_subscriber(shm_name, shm_size)
+    subscriber = SimStateSubscriber(shm_name, shm_size)
     subscriber.start_subscribe()
     return subscriber