ROS2 Integration

Overview

The sim.py script bridges the MuJoCo simulation with ROS2, enabling:

Real-time camera image streaming
Body state (position/orientation) publishing
Remote movement commands via topics
Simulation restart service

This allows you to control the robot from external nodes, integrate with ROS2 tools, and build distributed robotics applications.

Architecture

ROS2 Topics

Published Topics

robot_camera (sensor_msgs/Image)

Camera feed from robot’s perspective
640×480 RGB images at 10 Hz
Frame ID: robot_camera

body_state (std_msgs/Float32MultiArray)

Robot pose: [x, y, z, roll, pitch, yaw]
Updated at 10 Hz
Position in meters, angles in radians

Subscribed Topics

movement_command (std_msgs/Int32)

0: Stop (freeze gait)
1: Forward movement
2: Backward movement
Applied immediately to gait controller

ROS2 Services

restart_simulation (std_srvs/Trigger)

Resets MuJoCo simulation state
Resets gait controller phase
Returns success confirmation

Two-Terminal Setup

You need two terminals: one for the simulation, one for the GUI controller.

Terminal 1: Launch Simulation

Start the ROS2-enabled simulation:

Flat Terrain
Rough Terrain

cd ~/workspace/source
python3 sim.py --terrain flat

Output:

Loading world: model/world.xml (flat terrain)
[INFO] [robot_control_node]: Robot Control Node initialized

cd ~/workspace/source
python3 sim.py --terrain rough

Output:

Loading world: model/world_train.xml (rough terrain)
[INFO] [robot_control_node]: Robot Control Node initialized

The MuJoCo viewer opens and the robot starts in place (waiting for movement commands).

Terminal 2: Launch GUI Controller

In a new terminal, start the control GUI:

cd ~/workspace/source
python3 gui/gui.py

The PyQt5 GUI window opens with:

Login screen (credentials in gui/users.db)
Camera feed view
Gamepad control interface
Body state visualization

Control the Robot

Use the gamepad D-pad or joystick:

Up: Robot moves forward
Down: Robot moves backward
Release: Robot stops
Button 0: Restart simulation

Topic Details

Camera Images
Body State
Movement Commands

robot_camera Topic

Message Type: sensor_msgs/msg/ImagePublished by: sim.py (RobotControlNode)Frequency: 10 Hz (every 0.1 seconds)Specifications:

Resolution: 640×480 pixels
Encoding: rgb8 (24-bit color)
Frame ID: robot_camera
Source: MuJoCo offscreen renderer (sim.py:233)

Subscribing Example:

import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Image
from cv_bridge import CvBridge

class ImageSubscriber(Node):
    def __init__(self):
        super().__init__('image_subscriber')
        self.bridge = CvBridge()
        self.subscription = self.create_subscription(
            Image,
            'robot_camera',
            self.image_callback,
            10
        )
    
    def image_callback(self, msg):
        cv_image = self.bridge.imgmsg_to_cv2(msg, 'rgb8')
        # Process image...

body_state Topic

Message Type: std_msgs/msg/Float32MultiArrayPublished by: sim.py (RobotControlNode)Frequency: 10 HzData Format: [x, y, z, roll, pitch, yaw]

x, y, z: Position in meters (world frame)
roll, pitch, yaw: Orientation in radians (Euler XYZ)

Source: MuJoCo body sensors (sim.py:279-285)Subscribing Example:

from std_msgs.msg import Float32MultiArray

class StateSubscriber(Node):
    def __init__(self):
        super().__init__('state_subscriber')
        self.subscription = self.create_subscription(
            Float32MultiArray,
            'body_state',
            self.state_callback,
            10
        )
    
    def state_callback(self, msg):
        x, y, z, roll, pitch, yaw = msg.data
        print(f"Position: ({x:.3f}, {y:.3f}, {z:.3f})")
        print(f"Orientation: ({roll:.3f}, {pitch:.3f}, {yaw:.3f})")

movement_command Topic

Message Type: std_msgs/msg/Int32Subscribed by: sim.py (RobotControlNode)Command Values:

0: Stop (freeze gait, hold current position)
1: Move forward (normal gait progression)
2: Move backward (inverted gait direction)

Implementation: See sim.py:159-199Publishing Example:

from std_msgs.msg import Int32

class MovementPublisher(Node):
    def __init__(self):
        super().__init__('movement_publisher')
        self.publisher = self.create_publisher(
            Int32,
            'movement_command',
            10
        )
    
    def move_forward(self):
        msg = Int32()
        msg.data = 1
        self.publisher.publish(msg)
    
    def stop(self):
        msg = Int32()
        msg.data = 0
        self.publisher.publish(msg)

Services

restart_simulation Service

Service Type: std_srvs/srv/Trigger Server: sim.py (RobotControlNode) Behavior (sim.py:90-96):

Resets MuJoCo data (mj_resetData)
Resets gait controller phase
Resets camera capture timer
Returns success response

Calling Example:

from std_srvs.srv import Trigger

class RestartClient(Node):
    def __init__(self):
        super().__init__('restart_client')
        self.client = self.create_client(
            Trigger,
            'restart_simulation'
        )
    
    def call_restart(self):
        request = Trigger.Request()
        future = self.client.call_async(request)
        future.add_done_callback(self.response_callback)
    
    def response_callback(self, future):
        response = future.result()
        if response.success:
            print(f"Restart successful: {response.message}")

Debugging

Check Active Topics

List all active ROS2 topics:

ros2 topic list

Expected output:

/body_state
/movement_command
/parameter_events
/robot_camera
/rosout

Monitor Topic Data

Echo messages from a topic:

# View body state updates
ros2 topic echo /body_state

# View movement commands
ros2 topic echo /movement_command

# Check camera publishing rate
ros2 topic hz /robot_camera

Expected camera rate: ~10 Hz

Inspect Topic Info

Get detailed topic information:

ros2 topic info /robot_camera

Output:

Type: sensor_msgs/msg/Image
Publisher count: 1
Subscription count: 1

Test Services

List available services:

ros2 service list

Call restart service manually:

ros2 service call /restart_simulation std_srvs/srv/Trigger

Expected response:

success: True
message: 'Simulation restart requested'

Check Node Status

List running nodes:

ros2 node list

Expected nodes:

/gui_ros_node
/robot_control_node

View node info:

ros2 node info /robot_control_node

Common Issues

No Camera Images Received

Symptoms: GUI shows no camera feedChecks:

Verify topic is publishing:
```
ros2 topic hz /robot_camera
```
Check for errors in sim.py terminal

Ensure cv_bridge is installed:

pip install opencv-python
sudo apt install ros-humble-cv-bridge

Movement Commands Not Working

Symptoms: Robot doesn’t respond to gamepadChecks:

Verify commands are being published:
```
ros2 topic echo /movement_command
```

Check gamepad detection in GUI terminal:

Joystick detected: <gamepad_name>
Button 0 pressed

Ensure pygame is installed:
```
pip install pygame
```

Simulation Crashes on Restart

Symptoms: sim.py exits when calling restart serviceSolution: This is a known issue with viewer restarts. Current implementation handles in-place data reset (sim.py:248-258) without recreating the viewer.If crashes persist, restart both terminals manually.

Advanced Usage

Custom ROS2 Node

Create your own control node:

custom_controller.py

import rclpy
from rclpy.node import Node
from std_msgs.msg import Int32, Float32MultiArray

class CustomController(Node):
    def __init__(self):
        super().__init__('custom_controller')
        
        # Subscribe to robot state
        self.state_sub = self.create_subscription(
            Float32MultiArray,
            'body_state',
            self.state_callback,
            10
        )
        
        # Publish movement commands
        self.cmd_pub = self.create_publisher(
            Int32,
            'movement_command',
            10
        )
        
        # Simple forward controller
        self.timer = self.create_timer(1.0, self.control_loop)
    
    def state_callback(self, msg):
        x, y, z, roll, pitch, yaw = msg.data
        self.get_logger().info(f'Robot at: ({x:.2f}, {y:.2f}, {z:.2f})')
    
    def control_loop(self):
        # Send forward command
        msg = Int32()
        msg.data = 1
        self.cmd_pub.publish(msg)

def main():
    rclpy.init()
    node = CustomController()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

Run alongside sim.py:

python3 custom_controller.py

Get Started

Core Concepts

Usage Guides

Development

Overview

Architecture

ROS2 Topics

Published Topics

Subscribed Topics

ROS2 Services

Two-Terminal Setup

Topic Details

robot_camera Topic

body_state Topic

movement_command Topic

Services

restart_simulation Service

Debugging

Common Issues

Advanced Usage

Custom ROS2 Node

Next Steps

GUI Control

Baseline Testing

Build docs developers (and LLMs) love

Get Started

Core Concepts

Usage Guides

Development

​Overview

​Architecture

​ROS2 Topics

Published Topics

Subscribed Topics

​ROS2 Services

​Two-Terminal Setup

​Topic Details

​robot_camera Topic

​body_state Topic

​movement_command Topic

​Services

​restart_simulation Service

​Debugging

​Common Issues

​Advanced Usage

​Custom ROS2 Node

​Next Steps

GUI Control

Baseline Testing

Build docs developers (and LLMs) love

Overview

Architecture

ROS2 Topics

ROS2 Services

Two-Terminal Setup

Topic Details

robot_camera Topic

body_state Topic

movement_command Topic

Services

restart_simulation Service

Debugging

Common Issues

Advanced Usage

Custom ROS2 Node

Next Steps