From 1bbd9c8377f14ee3049fe5daf6b12a0d7ad797cd Mon Sep 17 00:00:00 2001 From: default Date: Fri, 2 Jan 2026 06:41:11 +0000 Subject: [PATCH] Gazebo Bridge Rover Update --- docs/installation.md | 21 +++++++++++++ gazebo/worlds/drone_landing.sdf | 35 +++++++++++++++------ gazebo_bridge.py | 55 +++++++++++++++++++++++++++++++-- 3 files changed, 99 insertions(+), 12 deletions(-) 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: