#!/usr/bin/env python3 """ Gazebo Bridge - GPS-denied interface with camera. Provides sensor data: IMU, altimeter, camera image. """ import base64 import json import math 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 nav_msgs.msg import Odometry from sensor_msgs.msg import Image from std_msgs.msg import String class GazeboBridge(Node): """Bridges Gazebo topics to GPS-denied sensor interface with camera.""" CAMERA_FOV = 60.0 CAMERA_RANGE = 10.0 CAMERA_WIDTH = 320 CAMERA_HEIGHT = 240 def __init__(self): super().__init__('gazebo_bridge') self.get_logger().info('=' * 60) self.get_logger().info('Gazebo Bridge Starting (GPS-Denied + Camera)...') self.get_logger().info('=' * 60) self._landed = False self._step_count = 0 self._rover_pos = [0.0, 0.0, 0.15] self._last_camera_image = None self._drone_pos = [0.0, 0.0, 5.0] self._drone_orn = [0.0, 0.0, 0.0, 1.0] sensor_qos = QoSProfile( reliability=ReliabilityPolicy.BEST_EFFORT, depth=10 ) self._cmd_vel_sub = self.create_subscription( Twist, '/cmd_vel', self._cmd_vel_callback, 10 ) self.get_logger().info(' Subscribed to: /cmd_vel') self._odom_sub = self.create_subscription( Odometry, '/model/drone/odometry', self._odom_callback, sensor_qos ) self.get_logger().info(' Subscribed to: /model/drone/odometry') self._camera_sub = self.create_subscription( Image, '/drone/camera', self._camera_callback, sensor_qos ) self.get_logger().info(' Subscribed to: /drone/camera') self._rover_sub = self.create_subscription( String, '/rover/telemetry', self._rover_callback, 10 ) self.get_logger().info(' Subscribed to: /rover/telemetry') self._gz_cmd_vel_pub = self.create_publisher( Twist, '/drone/cmd_vel', 10 ) self.get_logger().info(' Publishing to: /drone/cmd_vel (Gazebo)') self._telemetry_pub = self.create_publisher( String, '/drone/telemetry', 10 ) self.get_logger().info(' Publishing to: /drone/telemetry') self.get_logger().info('Gazebo Bridge Ready!') def _cmd_vel_callback(self, msg: Twist) -> None: gz_msg = Twist() gz_msg.linear.x = msg.linear.x gz_msg.linear.y = msg.linear.y gz_msg.linear.z = msg.linear.z gz_msg.angular.z = msg.angular.z self._gz_cmd_vel_pub.publish(gz_msg) def _rover_callback(self, msg: String) -> None: try: data = json.loads(msg.data) pos = data.get('position', {}) self._rover_pos = [ pos.get('x', 0.0), pos.get('y', 0.0), pos.get('z', 0.15) ] except json.JSONDecodeError: pass def _camera_callback(self, msg: Image) -> None: """Process camera images and encode as base64.""" try: if msg.encoding == 'rgb8': image = np.frombuffer(msg.data, dtype=np.uint8).reshape( msg.height, msg.width, 3 ) elif msg.encoding == 'bgr8': image = np.frombuffer(msg.data, dtype=np.uint8).reshape( msg.height, msg.width, 3 )[:, :, ::-1] else: return # Encode as base64 JPEG try: from PIL import Image as PILImage import io pil_img = PILImage.fromarray(image) buffer = io.BytesIO() pil_img.save(buffer, format='JPEG', quality=70) self._last_camera_image = base64.b64encode(buffer.getvalue()).decode('utf-8') except ImportError: self._last_camera_image = base64.b64encode(image.tobytes()).decode('utf-8') except Exception as e: self.get_logger().warning(f'Camera processing error: {e}') def _get_landing_pad_detection(self) -> Optional[Dict[str, float]]: dx = self._rover_pos[0] - self._drone_pos[0] dy = self._rover_pos[1] - self._drone_pos[1] dz = self._rover_pos[2] - self._drone_pos[2] horizontal_dist = math.sqrt(dx**2 + dy**2) vertical_dist = -dz if vertical_dist <= 0 or vertical_dist > self.CAMERA_RANGE: return None fov_rad = math.radians(self.CAMERA_FOV / 2) max_horizontal = vertical_dist * math.tan(fov_rad) if horizontal_dist > max_horizontal: return None roll, pitch, yaw = self._quaternion_to_euler( self._drone_orn[0], self._drone_orn[1], self._drone_orn[2], self._drone_orn[3] ) cos_yaw = math.cos(-yaw) sin_yaw = math.sin(-yaw) relative_x = dx * cos_yaw - dy * sin_yaw relative_y = dx * sin_yaw + dy * cos_yaw angle = math.atan2(horizontal_dist, vertical_dist) confidence = max(0.0, 1.0 - (angle / fov_rad)) confidence *= max(0.0, 1.0 - (vertical_dist / self.CAMERA_RANGE)) return { "relative_x": round(relative_x, 4), "relative_y": round(relative_y, 4), "distance": round(vertical_dist, 4), "confidence": round(confidence, 4) } def _odom_callback(self, msg: Odometry) -> None: self._step_count += 1 pos = msg.pose.pose.position vel = msg.twist.twist.linear ang_vel = msg.twist.twist.angular quat = msg.pose.pose.orientation self._drone_pos = [pos.x, pos.y, pos.z] self._drone_orn = [quat.x, quat.y, quat.z, quat.w] roll, pitch, yaw = self._quaternion_to_euler(quat.x, quat.y, quat.z, quat.w) landing_height = 0.5 self._landed = ( abs(pos.x - self._rover_pos[0]) < 0.5 and abs(pos.y - self._rover_pos[1]) < 0.5 and pos.z < landing_height and abs(vel.z) < 0.2 ) pad_detection = self._get_landing_pad_detection() telemetry = { "imu": { "orientation": { "roll": round(roll, 4), "pitch": round(pitch, 4), "yaw": round(yaw, 4) }, "angular_velocity": { "x": round(ang_vel.x, 4), "y": round(ang_vel.y, 4), "z": round(ang_vel.z, 4) }, "linear_acceleration": { "x": 0.0, "y": 0.0, "z": 9.81 } }, "altimeter": { "altitude": round(pos.z, 4), "vertical_velocity": round(vel.z, 4) }, "velocity": { "x": round(vel.x, 4), "y": round(vel.y, 4), "z": round(vel.z, 4) }, "landing_pad": pad_detection, "camera": { "width": self.CAMERA_WIDTH, "height": self.CAMERA_HEIGHT, "fov": self.CAMERA_FOV, "image": self._last_camera_image }, "landed": self._landed, "timestamp": round(self._step_count * 0.02, 4) } telemetry_msg = String() telemetry_msg.data = json.dumps(telemetry) self._telemetry_pub.publish(telemetry_msg) def _quaternion_to_euler(self, x: float, y: float, z: float, w: float) -> tuple: sinr_cosp = 2 * (w * x + y * z) cosr_cosp = 1 - 2 * (x * x + y * y) roll = math.atan2(sinr_cosp, cosr_cosp) sinp = 2 * (w * y - z * x) if abs(sinp) >= 1: pitch = math.copysign(math.pi / 2, sinp) else: pitch = math.asin(sinp) siny_cosp = 2 * (w * z + x * y) cosy_cosp = 1 - 2 * (y * y + z * z) yaw = math.atan2(siny_cosp, cosy_cosp) return roll, pitch, yaw def main(args=None): print("\n" + "=" * 60) print(" GAZEBO BRIDGE (GPS-DENIED + CAMERA)") print("=" * 60 + "\n") rclpy.init(args=args) bridge_node = None try: bridge_node = GazeboBridge() rclpy.spin(bridge_node) except KeyboardInterrupt: print('\nShutting down...') finally: if bridge_node is not None: bridge_node.destroy_node() if rclpy.ok(): rclpy.shutdown() if __name__ == '__main__': main()