Gazebo Bridge Rover Update

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: