ROS Bridge Update

ros_bridge.py

@@ -34,6 +34,9 @@ class ROS2SimulatorBridge(Node):
         self._simulator_port = simulator_port or self.DEFAULT_SIMULATOR_PORT
         self._telemetry_port = self._simulator_port + 1
         
+        # Rover position (updated from /rover/telemetry)
+        self._rover_pos = {'x': 0.0, 'y': 0.0, 'z': 0.15}
+        
         self.get_logger().info('=' * 60)
         self.get_logger().info('ROS 2 Simulator Bridge Starting...')
         self.get_logger().info(f'  Simulator Host: {self._simulator_host}')
@@ -50,6 +53,12 @@ class ROS2SimulatorBridge(Node):
         )
         self.get_logger().info(f'  Subscribed to: {self.CMD_VEL_TOPIC}')
         
+        # Subscribe to rover telemetry
+        self._rover_sub = self.create_subscription(
+            String, '/rover/telemetry', self._rover_callback, 10
+        )
+        self.get_logger().info('  Subscribed to: /rover/telemetry')
+        
         self._telemetry_pub = self.create_publisher(
             String, self.TELEMETRY_TOPIC, 10
         )
@@ -83,10 +92,24 @@ class ROS2SimulatorBridge(Node):
             'thrust': msg.linear.z,
             'yaw': msg.angular.z,
             'pitch': msg.linear.x,
-            'roll': msg.linear.y
+            'roll': msg.linear.y,
+            'rover': self._rover_pos  # Include current rover position
         }
         self._send_command_to_simulator(command)
 
+    def _rover_callback(self, msg: String) -> None:
+        """Update rover position from RoverController telemetry."""
+        try:
+            data = json.loads(msg.data)
+            pos = data.get('position', {})
+            self._rover_pos = {
+                'x': pos.get('x', 0.0),
+                'y': pos.get('y', 0.0),
+                'z': pos.get('z', 0.15)
+            }
+        except json.JSONDecodeError:
+            pass
+
     def _send_command_to_simulator(self, command: dict) -> bool:
         try:
             json_data = json.dumps(command).encode('utf-8')