Physical AI Systems — Architecture, Design, and Real-World Deployment

import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Image, PointCloud2
from vision_msgs.msg import Detection3DArray
import numpy as np

class PerceptionNode(Node):
    """Fuses camera and LiDAR data into 3D object detections."""

    def __init__(self):
        super().__init__('perception_node')

        # Subscribe to camera and LiDAR topics
        self.camera_sub = self.create_subscription(
            Image, '/camera/color/image_raw',
            self.camera_callback, 10)  # queue depth = 10

        self.lidar_sub = self.create_subscription(
            PointCloud2, '/lidar/points',
            self.lidar_callback, 10)

        # Publish fused 3D detections
        self.detection_pub = self.create_publisher(
            Detection3DArray, '/perception/detections', 10)

        # Load object detection model
        self.detector = self.load_model('yolov8_object_detection.onnx')
        self.get_logger().info('Perception node initialized')

    def camera_callback(self, msg):
        """Process camera frames at 30Hz."""
        # Convert ROS image to numpy array
        image = self.ros_image_to_numpy(msg)

        # Run object detection
        detections_2d = self.detector.infer(image)

        # Store with timestamp for fusion with LiDAR
        self.latest_camera_detections = (msg.header.stamp, detections_2d)

    def lidar_callback(self, msg):
        """Process LiDAR point cloud at 10Hz and fuse with camera."""
        points = self.pointcloud2_to_numpy(msg)

        if hasattr(self, 'latest_camera_detections'):
            # Project detections from 2D image space to 3D world space
            detections_3d = self.fuse_camera_lidar(
                self.latest_camera_detections[1], points)

            # Publish fused 3D detections
            detection_msg = self.create_detection_msg(detections_3d, msg.header)
            self.detection_pub.publish(detection_msg)

def main(args=None):
    rclpy.init(args=args)
    node = PerceptionNode()
    rclpy.spin(node)   # keep node running, processing callbacks
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()