Gazebo Bridge Rover Update

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:

gazebo/worlds/drone_landing.sdf

@@ -1,10 +1,10 @@
 <?xml version="1.0" ?>
 <!--
-  Drone Landing Competition - Gazebo World
+  Drone Landing Simulation - Gazebo World
   
   This world contains:
   - Ground plane with grid texture
-  - Landing pad (rover) at origin
+  - Landing pad (rover) at origin - can be moved via pose commands
   - Drone spawned at 5m altitude
   - Sun for lighting
 -->
@@ -18,13 +18,13 @@
     </physics>
     
     <!-- Required plugins -->
-    <plugin filename="gz-sim-physics-system" name="gz::sim::systems::Physics"/>
+    <plugin filename="ignition-gazebo-physics-system" name="ignition::gazebo::systems::Physics"/>
-    <plugin filename="gz-sim-user-commands-system" name="gz::sim::systems::UserCommands"/>
+    <plugin filename="ignition-gazebo-user-commands-system" name="ignition::gazebo::systems::UserCommands"/>
-    <plugin filename="gz-sim-scene-broadcaster-system" name="gz::sim::systems::SceneBroadcaster"/>
+    <plugin filename="ignition-gazebo-scene-broadcaster-system" name="ignition::gazebo::systems::SceneBroadcaster"/>
-    <plugin filename="gz-sim-sensors-system" name="gz::sim::systems::Sensors">
+    <plugin filename="ignition-gazebo-sensors-system" name="ignition::gazebo::systems::Sensors">
       <render_engine>ogre2</render_engine>
     </plugin>
-    <plugin filename="gz-sim-imu-system" name="gz::sim::systems::Imu"/>
+    <plugin filename="ignition-gazebo-imu-system" name="ignition::gazebo::systems::Imu"/>
     
     <!-- Lighting -->
     <light type="directional" name="sun">
@@ -69,11 +69,28 @@
       </link>
     </model>
     
-    <!-- Landing Pad (Rover) -->
+    <!-- Landing Pad (Rover) - Non-static so it can be moved -->
     <model name="landing_pad">
-      <static>true</static>
+      <static>false</static>
       <pose>0 0 0.15 0 0 0</pose>
+      
+      <!-- Pose publisher to track position -->
+      <plugin filename="ignition-gazebo-pose-publisher-system" name="ignition::gazebo::systems::PosePublisher">
+        <publish_link_pose>false</publish_link_pose>
+        <publish_model_pose>true</publish_model_pose>
+        <publish_nested_model_pose>false</publish_nested_model_pose>
+        <update_frequency>50</update_frequency>
+      </plugin>
+      
       <link name="base_link">
+        <inertial>
+          <mass>100.0</mass>
+          <inertia>
+            <ixx>10.0</ixx>
+            <iyy>10.0</iyy>
+            <izz>10.0</izz>
+          </inertia>
+        </inertial>
         <collision name="collision">
           <geometry>
             <box>

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: