ROS Bridge Update

gazebo/worlds/drone_landing.sdf

@@ -1,47 +1,29 @@
 <?xml version="1.0" ?>
 <!--
-  Drone Landing Simulation - Gazebo World
+  Drone Landing Simulation - Gazebo World (Ignition Fortress compatible)
-  
-  This world contains:
-  - Ground plane with grid texture
-  - Landing pad (rover) at origin - can be moved via pose commands
-  - Drone spawned at 5m altitude
-  - Sun for lighting
 -->
 <sdf version="1.8">
   <world name="drone_landing_world">
     
-    <!-- Physics configuration -->
     <physics name="1ms" type="ignored">
       <max_step_size>0.001</max_step_size>
       <real_time_factor>1.0</real_time_factor>
     </physics>
     
-    <!-- Required plugins -->
+    <!-- Ignition Fortress plugins -->
-    <plugin filename="ignition-gazebo-physics-system" name="ignition::gazebo::systems::Physics"/>
+    <plugin filename="libignition-gazebo-physics-system.so" name="ignition::gazebo::systems::Physics"/>
-    <plugin filename="ignition-gazebo-user-commands-system" name="ignition::gazebo::systems::UserCommands"/>
+    <plugin filename="libignition-gazebo-user-commands-system.so" name="ignition::gazebo::systems::UserCommands"/>
-    <plugin filename="ignition-gazebo-scene-broadcaster-system" name="ignition::gazebo::systems::SceneBroadcaster"/>
+    <plugin filename="libignition-gazebo-scene-broadcaster-system.so" name="ignition::gazebo::systems::SceneBroadcaster"/>
-    <plugin filename="ignition-gazebo-sensors-system" name="ignition::gazebo::systems::Sensors">
-      <render_engine>ogre2</render_engine>
-    </plugin>
-    <plugin filename="ignition-gazebo-imu-system" name="ignition::gazebo::systems::Imu"/>
     
-    <!-- Lighting -->
     <light type="directional" name="sun">
       <cast_shadows>true</cast_shadows>
       <pose>0 0 10 0 0 0</pose>
       <diffuse>0.8 0.8 0.8 1</diffuse>
       <specular>0.2 0.2 0.2 1</specular>
-      <attenuation>
-        <range>1000</range>
-        <constant>0.9</constant>
-        <linear>0.01</linear>
-        <quadratic>0.001</quadratic>
-      </attenuation>
       <direction>-0.5 0.1 -0.9</direction>
     </light>
     
-    <!-- Ground Plane -->
+    <!-- Ground -->
     <model name="ground_plane">
       <static>true</static>
       <link name="link">
@@ -61,43 +43,18 @@
             </plane>
           </geometry>
           <material>
-            <ambient>0.3 0.3 0.3 1</ambient>
+            <ambient>0.5 0.5 0.5 1</ambient>
             <diffuse>0.7 0.7 0.7 1</diffuse>
-            <specular>0.01 0.01 0.01 1</specular>
           </material>
         </visual>
       </link>
     </model>
     
-    <!-- Landing Pad (Rover) - Non-static so it can be moved -->
+    <!-- Landing Pad (static for now) -->
     <model name="landing_pad">
-      <static>false</static>
+      <static>true</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>
-              <size>1.0 1.0 0.3</size>
-            </box>
-          </geometry>
-        </collision>
         <visual name="visual">
           <geometry>
             <box>
@@ -105,49 +62,17 @@
             </box>
           </geometry>
           <material>
-            <ambient>0.1 0.4 0.1 1</ambient>
+            <ambient>0.1 0.5 0.1 1</ambient>
-            <diffuse>0.2 0.6 0.2 1</diffuse>
+            <diffuse>0.2 0.7 0.2 1</diffuse>
-            <specular>0.1 0.1 0.1 1</specular>
           </material>
         </visual>
-        
+        <collision name="collision">
-        <!-- Landing pad "H" marker -->
-        <visual name="h_marker">
-          <pose>0 0 0.151 0 0 0</pose>
           <geometry>
             <box>
-              <size>0.6 0.1 0.001</size>
+              <size>1.0 1.0 0.3</size>
             </box>
           </geometry>
-          <material>
+        </collision>
-            <ambient>1 1 1 1</ambient>
-            <diffuse>1 1 1 1</diffuse>
-          </material>
-        </visual>
-        <visual name="h_left">
-          <pose>-0.25 0 0.151 0 0 0</pose>
-          <geometry>
-            <box>
-              <size>0.1 0.6 0.001</size>
-            </box>
-          </geometry>
-          <material>
-            <ambient>1 1 1 1</ambient>
-            <diffuse>1 1 1 1</diffuse>
-          </material>
-        </visual>
-        <visual name="h_right">
-          <pose>0.25 0 0.151 0 0 0</pose>
-          <geometry>
-            <box>
-              <size>0.1 0.6 0.001</size>
-            </box>
-          </geometry>
-          <material>
-            <ambient>1 1 1 1</ambient>
-            <diffuse>1 1 1 1</diffuse>
-          </material>
-        </visual>
       </link>
     </model>
     

gazebo_bridge.py

@@ -8,8 +8,6 @@ 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
@@ -44,11 +42,6 @@ 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
@@ -95,57 +88,23 @@ 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."""
+        """Receive rover position (for telemetry only - rover is static in Gazebo)."""
         try:
             data = json.loads(msg.data)
             pos = data.get('position', {})
-            new_pos = [
+            self._rover_pos = [
                 pos.get('x', 0.0),
                 pos.get('y', 0.0),
                 pos.get('z', 0.15)
             ]
-            
+            # Note: In Gazebo mode, rover position is tracked but the visual
-            # Only update if position changed significantly
+            # model stays static. Moving the model requires the set_pose service
-            if (abs(new_pos[0] - self._rover_pos[0]) > 0.01 or
+            # which has networking issues in WSL2. For moving rover tests,
-                abs(new_pos[1] - self._rover_pos[1]) > 0.01):
+            # use PyBullet (standalone_simulation.py or simulation_host.py).
-                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."""

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')

simulation_host.py

@@ -208,12 +208,28 @@ class DroneSimulator:
             
             try:
                 command = json.loads(data.decode('utf-8'))
+                
+                # Drone control commands
                 self._last_command = {
                     'thrust': float(command.get('thrust', 0.0)),
                     'pitch': float(command.get('pitch', 0.0)),
                     'roll': float(command.get('roll', 0.0)),
                     'yaw': float(command.get('yaw', 0.0))
                 }
+                
+                # Rover position update (if provided)
+                if 'rover' in command:
+                    rover_data = command['rover']
+                    new_x = float(rover_data.get('x', self._rover_pos[0]))
+                    new_y = float(rover_data.get('y', self._rover_pos[1]))
+                    new_z = float(rover_data.get('z', self._rover_pos[2]))
+                    self._rover_pos = [new_x, new_y, new_z]
+                    p.resetBasePositionAndOrientation(
+                        self._rover_id,
+                        self._rover_pos,
+                        [0, 0, 0, 1]
+                    )
+                    
             except (json.JSONDecodeError, ValueError) as e:
                 print(f"Invalid command received: {e}")