diff --git a/docs/installation.md b/docs/installation.md
index e0d948d..3d819d8 100644
--- a/docs/installation.md
+++ b/docs/installation.md
@@ -239,6 +239,27 @@ ign gazebo --version # Fortress (ROS 2 Humble)
**Note:** ROS 2 Humble uses Gazebo Fortress, which uses `ign gazebo` command instead of `gz sim`. The launch file auto-detects which command is available.
+**Gazebo GPU Issues in WSL2:**
+
+If Gazebo crashes with GPU/OpenGL errors, try:
+
+```bash
+# Option 1: Run in server mode (no GUI)
+ign gazebo -s gazebo/worlds/drone_landing.sdf
+
+# Option 2: Fix permissions and restart WSL
+sudo usermod -aG render $USER
+chmod 700 /run/user/1000
+# Then in PowerShell: wsl --shutdown
+
+# Option 3: Force software rendering
+export LIBGL_ALWAYS_SOFTWARE=1
+ign gazebo gazebo/worlds/drone_landing.sdf
+
+# Option 4: Just use PyBullet (more reliable on WSL2)
+python standalone_simulation.py
+```
+
**Troubleshooting WSL GUI:**
If GUI doesn't work:
diff --git a/gazebo/worlds/drone_landing.sdf b/gazebo/worlds/drone_landing.sdf
index 3b55d36..785aedf 100644
--- a/gazebo/worlds/drone_landing.sdf
+++ b/gazebo/worlds/drone_landing.sdf
@@ -1,10 +1,10 @@
@@ -18,13 +18,13 @@
-
-
-
-
+
+
+
+
ogre2
-
+
@@ -69,11 +69,28 @@
-
+
- true
+ false
0 0 0.15 0 0 0
+
+
+
+ false
+ true
+ false
+ 50
+
+
+
+ 100.0
+
+ 10.0
+ 10.0
+ 10.0
+
+
diff --git a/gazebo_bridge.py b/gazebo_bridge.py
index 231a6a0..2904dd7 100644
--- a/gazebo_bridge.py
+++ b/gazebo_bridge.py
@@ -1,19 +1,22 @@
#!/usr/bin/env python3
"""
-Gazebo Bridge - GPS-denied interface with camera.
+Gazebo Bridge - GPS-denied interface with camera and rover control.
Provides sensor data: IMU, altimeter, camera image.
+Controls rover movement via Gazebo pose commands.
"""
import base64
import json
import math
+import subprocess
+import threading
from typing import Optional, Dict, Any
import numpy as np
import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile, ReliabilityPolicy
-from geometry_msgs.msg import Twist
+from geometry_msgs.msg import Twist, Pose
from nav_msgs.msg import Odometry
from sensor_msgs.msg import Image
from std_msgs.msg import String
@@ -41,6 +44,11 @@ class GazeboBridge(Node):
self._drone_pos = [0.0, 0.0, 5.0]
self._drone_orn = [0.0, 0.0, 0.0, 1.0]
+ # Detect gz vs ign command
+ import shutil
+ self._gz_cmd = 'gz' if shutil.which('gz') else 'ign'
+ self.get_logger().info(f' Using Gazebo command: {self._gz_cmd}')
+
sensor_qos = QoSProfile(
reliability=ReliabilityPolicy.BEST_EFFORT,
depth=10
@@ -87,17 +95,58 @@ class GazeboBridge(Node):
self._gz_cmd_vel_pub.publish(gz_msg)
def _rover_callback(self, msg: String) -> None:
+ """Receive rover position and update in Gazebo."""
try:
data = json.loads(msg.data)
pos = data.get('position', {})
- self._rover_pos = [
+ new_pos = [
pos.get('x', 0.0),
pos.get('y', 0.0),
pos.get('z', 0.15)
]
+
+ # Only update if position changed significantly
+ if (abs(new_pos[0] - self._rover_pos[0]) > 0.01 or
+ abs(new_pos[1] - self._rover_pos[1]) > 0.01):
+ self._rover_pos = new_pos
+ # Move rover in Gazebo (async to avoid blocking)
+ threading.Thread(target=self._move_rover_in_gazebo, daemon=True).start()
+
except json.JSONDecodeError:
pass
+ def _move_rover_in_gazebo(self) -> None:
+ """Send pose command to Gazebo to move the landing pad."""
+ try:
+ x, y, z = self._rover_pos
+
+ # Use Gazebo service to set model pose
+ if self._gz_cmd == 'ign':
+ # Ignition Gazebo (Fortress)
+ cmd = [
+ 'ign', 'service', '-s', '/world/drone_landing_world/set_pose',
+ '--reqtype', 'ignition.msgs.Pose',
+ '--reptype', 'ignition.msgs.Boolean',
+ '--timeout', '100',
+ '--req', f'name: "landing_pad", position: {{x: {x}, y: {y}, z: {z}}}'
+ ]
+ else:
+ # Newer Gazebo
+ cmd = [
+ 'gz', 'service', '-s', '/world/drone_landing_world/set_pose',
+ '--reqtype', 'gz.msgs.Pose',
+ '--reptype', 'gz.msgs.Boolean',
+ '--timeout', '100',
+ '--req', f'name: "landing_pad", position: {{x: {x}, y: {y}, z: {z}}}'
+ ]
+
+ subprocess.run(cmd, capture_output=True, timeout=1.0)
+
+ except subprocess.TimeoutExpired:
+ pass
+ except Exception as e:
+ pass # Silently ignore errors to avoid spam
+
def _camera_callback(self, msg: Image) -> None:
"""Process camera images and encode as base64."""
try: