From 924d803e71fca12953e61077291d7aa778487568 Mon Sep 17 00:00:00 2001 From: default Date: Fri, 2 Jan 2026 06:50:31 +0000 Subject: [PATCH] ROS Bridge Update --- gazebo/worlds/drone_landing.sdf | 103 +++++--------------------------- gazebo_bridge.py | 53 ++-------------- ros_bridge.py | 25 +++++++- simulation_host.py | 16 +++++ 4 files changed, 60 insertions(+), 137 deletions(-) diff --git a/gazebo/worlds/drone_landing.sdf b/gazebo/worlds/drone_landing.sdf index 785aedf..a80a200 100644 --- a/gazebo/worlds/drone_landing.sdf +++ b/gazebo/worlds/drone_landing.sdf @@ -1,47 +1,29 @@ - 0.001 1.0 - - - - - - ogre2 - - + + + + - true 0 0 10 0 0 0 0.8 0.8 0.8 1 0.2 0.2 0.2 1 - - 1000 - 0.9 - 0.01 - 0.001 - -0.5 0.1 -0.9 - + true @@ -61,43 +43,18 @@ - 0.3 0.3 0.3 1 + 0.5 0.5 0.5 1 0.7 0.7 0.7 1 - 0.01 0.01 0.01 1 - + - false + true 0 0 0.15 0 0 0 - - - - false - true - false - 50 - - - - 100.0 - - 10.0 - 10.0 - 10.0 - - - - - - 1.0 1.0 0.3 - - - @@ -105,49 +62,17 @@ - 0.1 0.4 0.1 1 - 0.2 0.6 0.2 1 - 0.1 0.1 0.1 1 + 0.1 0.5 0.1 1 + 0.2 0.7 0.2 1 - - - - 0 0 0.151 0 0 0 + - 0.6 0.1 0.001 + 1.0 1.0 0.3 - - 1 1 1 1 - 1 1 1 1 - - - - -0.25 0 0.151 0 0 0 - - - 0.1 0.6 0.001 - - - - 1 1 1 1 - 1 1 1 1 - - - - 0.25 0 0.151 0 0 0 - - - 0.1 0.6 0.001 - - - - 1 1 1 1 - 1 1 1 1 - - + diff --git a/gazebo_bridge.py b/gazebo_bridge.py index 2904dd7..08b1321 100644 --- a/gazebo_bridge.py +++ b/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) ] - - # 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() + # Note: In Gazebo mode, rover position is tracked but the visual + # model stays static. Moving the model requires the set_pose service + # which has networking issues in WSL2. For moving rover tests, + # use PyBullet (standalone_simulation.py or simulation_host.py). 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.""" diff --git a/ros_bridge.py b/ros_bridge.py index d9f6ffc..d3ebf9b 100644 --- a/ros_bridge.py +++ b/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') diff --git a/simulation_host.py b/simulation_host.py index bc6d0c7..3837086 100644 --- a/simulation_host.py +++ b/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}")