feat: Add ROS2 simulation support

__init__.py

@@ -0,0 +1 @@
+"""Project-local Unitree MuJoCo utilities."""

readme.md

@@ -121,6 +121,63 @@ The program will output the robot's pose and position information in the simulat
 
 **Note:** The testing program sends the unitree_go message. If you want to test G1 robot, you need to modify the program to use the unitree_hg message.
 
+## Python ROS2 Simulator (simulate_python_ros2)
+This simulator is a ROS2-native Python entrypoint for G1 only. It does not use `unitree_sdk2_python` or DDS `rt/*` channels.
+
+### 1. Dependencies
+#### unitree_ros2
+Build and source the Unitree ROS2 workspace first:
+```bash
+source ~/unitree_ros2/setup.sh
+```
+The simulator depends on `rclpy`, `unitree_hg`, and `unitree_go` from that workspace.
+
+#### mujoco-python
+```bash
+pip3 install mujoco
+```
+
+#### joystick
+Optional. If `pygame` is unavailable or no controller is connected, the simulator still runs and publishes zero-valued `/wirelesscontroller`.
+```bash
+pip3 install pygame
+```
+
+### 2. Run
+```bash
+cd ./simulate_python_ros2
+python3 ./unitree_mujoco_ros2.py
+```
+
+This version publishes these ROS2 topics directly:
+- `/lowstate` -> `unitree_hg/msg/LowState`
+- `/secondary_imu` -> `unitree_hg/msg/IMUState`
+- `/lf/bmsstate` -> `unitree_hg/msg/BmsState`
+- `/wirelesscontroller` -> `unitree_go/msg/WirelessController`
+- `/sportmodestate` -> `unitree_go/msg/SportModeState`
+
+It subscribes to:
+- `/lowcmd` -> `unitree_hg/msg/LowCmd`
+
+The provided smoke script checks that the expected topics have data:
+```bash
+python3 ./test/smoke_ros2_bridge.py
+```
+
+### 3. Config
+The ROS2-native Python simulator configuration file is at `/simulate_python_ros2/config.py`:
+```python
+ROBOT = "g1"
+ROBOT_SCENE = "../unitree_robots/" + ROBOT + "/scene_29dof.xml"
+USE_JOYSTICK = 1
+JOYSTICK_TYPE = "xbox"
+JOYSTICK_DEVICE = 0
+PRINT_SCENE_INFORMATION = True
+ENABLE_ELASTIC_BAND = False
+SIMULATE_DT = 0.005
+VIEWER_DT = 0.02
+```
+
 
 # Usage
 ## 1. Simulation Configuration
@@ -307,4 +364,4 @@ export ROS_DOMAIN_ID=1 # Modify the domain id to match the simulation
 source ~/unitree_ros2/setup.sh # Use the network card connected to the robot
 export ROS_DOMAIN_ID=0 # Use the default domain id
 ./install/stand_go2/bin/stand_go2 # Run
-```
+```

readme_zh.md

@@ -184,6 +184,61 @@ SIMULATE_DT = 0.003
 VIEWER_DT = 0.02 
 ```
 
+### Python ROS2 仿真器 (`simulate_python_ros2`)
+这是一个仅面向 G1 的 ROS2 Native Python 版本，不使用 `unitree_sdk2_python`，也不走 DDS `rt/*` 接口。
+
+#### 1. 依赖
+首先需要正确 source `unitree_ros2` 工作区：
+```bash
+source ~/unitree_ros2/setup.sh
+```
+这个版本直接依赖该工作区提供的 `rclpy`、`unitree_hg`、`unitree_go` Python 包。
+
+MuJoCo 依赖：
+```bash
+pip3 install mujoco
+```
+
+手柄依赖是可选的；如果没有 `pygame` 或没有物理手柄，仿真器仍会运行，并持续发布全 0 的 `/wirelesscontroller`：
+```bash
+pip3 install pygame
+```
+
+#### 2. 启动
+```bash
+cd ./simulate_python_ros2
+python3 ./unitree_mujoco_ros2.py
+```
+
+该版本直接发布以下 ROS2 话题：
+- `/lowstate` -> `unitree_hg/msg/LowState`
+- `/secondary_imu` -> `unitree_hg/msg/IMUState`
+- `/lf/bmsstate` -> `unitree_hg/msg/BmsState`
+- `/wirelesscontroller` -> `unitree_go/msg/WirelessController`
+- `/sportmodestate` -> `unitree_go/msg/SportModeState`
+
+订阅：
+- `/lowcmd` -> `unitree_hg/msg/LowCmd`
+
+可以用附带的 smoke 脚本做最小检查：
+```bash
+python3 ./test/smoke_ros2_bridge.py
+```
+
+#### 3. 配置
+ROS2 Native Python 仿真器的配置文件位于 `/simulate_python_ros2/config.py`：
+```python
+ROBOT = "g1"
+ROBOT_SCENE = "../unitree_robots/" + ROBOT + "/scene_29dof.xml"
+USE_JOYSTICK = 1
+JOYSTICK_TYPE = "xbox"
+JOYSTICK_DEVICE = 0
+PRINT_SCENE_INFORMATION = True
+ENABLE_ELASTIC_BAND = False
+SIMULATE_DT = 0.005
+VIEWER_DT = 0.02
+```
+
 ### 游戏手柄
 仿真器会使用 Xbox 或者 Switch 游戏来模拟机器人的无线控制器，并将手柄按键和摇杆信息发布在"rt/wireless_controller" topic。如果手上没有可以使用的游戏手柄，需要将 `config.yaml/config.py` 中的 `use_joystick/USE_JOYSTICK` 设置为 0。如果使用的手柄不属于 Xbox 和 Switch 映射，可以在源码中自行修改或添加(可以使用 `jstest` 工具查看按键和摇杆 id)：
 
@@ -315,4 +370,4 @@ export ROS_DOMAIN_ID=1 # 修改domain id 与仿真一致
 source ~/unitree_ros2/setup.sh # 使用机器人连接的网卡
 export ROS_DOMAIN_ID=0 # 使用默认的 domain id 
 ./install/stand_go2/bin/stand_go2 # 运行
-```
+```

simulate_python_ros2/__init__.py

@@ -0,0 +1 @@
+"""ROS2-native Python simulator bridge for Unitree MuJoCo."""

simulate_python_ros2/config.py

@@ -0,0 +1,12 @@
+ROBOT = "g1"  # Only G1 is supported in the ROS2-native simulator.
+ROBOT_SCENE = "../unitree_robots/" + ROBOT + "/scene_29dof.xml"  # Default to the G1 29DoF scene.
+
+USE_JOYSTICK = 1  # Simulate Unitree WirelessController using a gamepad
+JOYSTICK_TYPE = "xbox"  # support "xbox" and "switch" gamepad layout
+JOYSTICK_DEVICE = 0  # Joystick number
+
+PRINT_SCENE_INFORMATION = True  # Print link, joint and sensors information of robot
+ENABLE_ELASTIC_BAND = True  # Virtual spring band, used for lifting h1
+
+SIMULATE_DT = 0.005  # Need to be larger than the runtime of viewer.sync()
+VIEWER_DT = 0.02  # 50 fps for viewer

simulate_python_ros2/test/smoke_ros2_bridge.py

@@ -0,0 +1,43 @@
+import argparse
+import subprocess
+import sys
+import time
+
+
+TOPICS = (
+    "/lowstate",
+    "/secondary_imu",
+    "/wirelesscontroller",
+    "/lf/bmsstate",
+    "/sportmodestate",
+)
+
+
+def main():
+    parser = argparse.ArgumentParser(description="Smoke-check expected ROS2 topics from simulate_python_ros2.")
+    parser.add_argument("--timeout", type=float, default=5.0, help="Timeout per topic echo in seconds.")
+    args = parser.parse_args()
+
+    failures = []
+    for topic in TOPICS:
+        cmd = ["ros2", "topic", "echo", "--once", topic]
+        try:
+            result = subprocess.run(cmd, capture_output=True, text=True, timeout=args.timeout)
+        except subprocess.TimeoutExpired:
+            failures.append(f"{topic}: timeout after {args.timeout}s")
+            continue
+        if result.returncode != 0:
+            failures.append(f"{topic}: exit {result.returncode}: {result.stderr.strip()}")
+            continue
+        sys.stdout.write(f"[OK] {topic}\n")
+        sys.stdout.flush()
+        time.sleep(0.1)
+
+    if failures:
+        for failure in failures:
+            sys.stderr.write(f"[FAIL] {failure}\n")
+        raise SystemExit(1)
+
+
+if __name__ == "__main__":
+    main()

simulate_python_ros2/unitree_mujoco_ros2.py

@@ -0,0 +1,88 @@
+import threading
+import time
+from threading import Thread
+
+import mujoco
+import mujoco.viewer
+import rclpy
+
+try:
+    from . import config
+    from .unitree_ros2_bridge import ElasticBand, UnitreeRos2Bridge
+except ImportError:
+    import config
+    from unitree_ros2_bridge import ElasticBand, UnitreeRos2Bridge
+
+
+def main():
+    if config.ROBOT != "g1":
+        raise ValueError("simulate_python_ros2 only supports config.ROBOT == 'g1'")
+
+    locker = threading.Lock()
+    mj_model = mujoco.MjModel.from_xml_path(config.ROBOT_SCENE)
+    mj_data = mujoco.MjData(mj_model)
+    mj_model.opt.timestep = config.SIMULATE_DT
+
+    if config.ENABLE_ELASTIC_BAND:
+        elastic_band = ElasticBand()
+        band_attached_link = mj_model.body("torso_link").id
+        viewer = mujoco.viewer.launch_passive(
+            mj_model, mj_data, key_callback=elastic_band.MujuocoKeyCallback
+        )
+    else:
+        elastic_band = None
+        band_attached_link = None
+        viewer = mujoco.viewer.launch_passive(mj_model, mj_data)
+
+    time.sleep(0.2)
+
+    def simulation_thread():
+        rclpy.init()
+        bridge = UnitreeRos2Bridge(
+            mj_model,
+            mj_data,
+            config.ROBOT_SCENE,
+            use_joystick=config.USE_JOYSTICK,
+            joystick_type=config.JOYSTICK_TYPE,
+            joystick_device=config.JOYSTICK_DEVICE,
+        )
+
+        if config.PRINT_SCENE_INFORMATION:
+            print("ROS2-native G1 simulator running with scene:", config.ROBOT_SCENE)
+
+        try:
+            while viewer.is_running():
+                step_start = time.perf_counter()
+                rclpy.spin_once(bridge, timeout_sec=0.0)
+
+                with locker:
+                    bridge.apply_latest_command()
+                    if config.ENABLE_ELASTIC_BAND and elastic_band is not None and elastic_band.enable:
+                        mj_data.xfrc_applied[band_attached_link, :3] = elastic_band.Advance(
+                            mj_data.qpos[:3], mj_data.qvel[:3]
+                        )
+                    mujoco.mj_step(mj_model, mj_data)
+                    bridge.publish_step_messages()
+
+                time_until_next_step = mj_model.opt.timestep - (time.perf_counter() - step_start)
+                if time_until_next_step > 0:
+                    time.sleep(time_until_next_step)
+        finally:
+            bridge.destroy_node()
+            rclpy.shutdown()
+
+    def physics_viewer_thread():
+        while viewer.is_running():
+            with locker:
+                viewer.sync()
+            time.sleep(config.VIEWER_DT)
+
+    viewer_thread = Thread(target=physics_viewer_thread)
+    sim_thread = Thread(target=simulation_thread)
+
+    viewer_thread.start()
+    sim_thread.start()
+
+
+if __name__ == "__main__":
+    main()