From 1388d3311af2468dc228b68b82fc4d7456b005f6 Mon Sep 17 00:00:00 2001 From: silencht Date: Tue, 8 Jul 2025 15:55:03 +0800 Subject: [PATCH] [fix] bug. --- README.md | 7 +-- README_ja-JP.md | 2 +- teleop/teleop_hand_and_arm.py | 8 +-- teleop/utils/episode_writer.py | 2 +- teleop/utils/sim_state_topic.py | 92 ++++++++++++--------------------- 5 files changed, 43 insertions(+), 68 deletions(-) diff --git a/README.md b/README.md index 66aca15..ed612c7 100644 --- a/README.md +++ b/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**
After pressing **r** to start, press **s** to start/stop saving an episode. Can repeat. | +| `--motion` | Enable **motion mode**
After enabling this mode, the teleoperation program can run alongside the robot's motion control.
In **hand tracking** mode, you can use the [R3 Controller](https://www.unitree.com/cn/R3) to control the robot's walking behavior;
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. diff --git a/README_ja-JP.md b/README_ja-JP.md index f77c001..7132c2f 100644 --- a/README_ja-JP.md +++ b/README_ja-JP.md @@ -183,7 +183,7 @@ G1(29 DoF)とDex3ハンド構成でシミュレーションを起動: | ⚙️ フラグ | 📜 説明 | | :----------: | :----------------------------------------------------------: | | `--record` | **データ記録有効化**: **r**押下で開始後、**s**でエピソード記録開始/停止。繰り返し可能 | -| `--motion` | **モーション制御有効化**: 遠隔操作中に独立したロボット制御を許可。ハンドモードではR3リモコンで歩行、コントローラーモードではジョイスティックで歩行 | +| `--motion` | **モーション制御有効化**: 遠隔操作中に独立したロボット制御を許可。
ハンドモードではR3リモコンで歩行、コントローラーモードではジョイスティックで歩行 | | `--headless` | GUIなしで実行（ヘッドレスPC2展開用） | | `--sim` | **シミュレーションモード**有効化 | diff --git a/teleop/teleop_hand_and_arm.py b/teleop/teleop_hand_and_arm.py index 5ed2c6a..411ba11 100644 --- a/teleop/teleop_hand_and_arm.py +++ b/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) diff --git a/teleop/utils/episode_writer.py b/teleop/utils/episode_writer.py index ea59faf..2dbb07c 100644 --- a/teleop/utils/episode_writer.py +++ b/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 diff --git a/teleop/utils/sim_state_topic.py b/teleop/utils/sim_state_topic.py index 0dd7947..c2bd498 100644 --- a/teleop/utils/sim_state_topic.py +++ b/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