linerobot/Unity/LineRobot/Assets/CameraJpegPubExistingTexture.cs

using System.Collections;
using System.Collections.Generic;
using UnityEngine;
using UnityEngine.Rendering;
using Unity.Collections;
using ROS2;

/*
 * This class is the same as CameraJpegPub only the rendertexture is already defined and assigned to the Camera 
 * rendertexture MUST be R8G8B8_UNORM
 */
[RequireComponent(typeof(Camera))]
public class CameraJpegPubExistingTexture : MonoBehaviour
{
    private Queue<AsyncGPUReadbackRequest> requests = new Queue<AsyncGPUReadbackRequest>();

    public float timeBetweenRequests = 0.2f;
    public string TopicName = "rt/jaja/image_raw/compressed";
    public string frame_id = null;
    [Range(0, 100)]
    public int JpgQuality = 50;

    private Camera MyCamera;
    private Texture2D tex;
    private RenderTexture m_RenderTexture;
    private float timer = 0.0f;
    private float[] depth_result;

    private ROS2UnityComponent ros2Unity;
    private ROS2Node ros2Node;
    private IPublisher<sensor_msgs.msg.CompressedImage> chatter_pub;

    void Start()
    {
        ros2Unity = GetComponent<ROS2UnityComponent>();
        if (ros2Unity.Ok())
        {
            if (ros2Node == null)
            {
                ros2Node = ros2Unity.CreateNode("ROS2LFRcamera");
                chatter_pub = ros2Node.CreatePublisher<sensor_msgs.msg.CompressedImage>(TopicName);
            }
        }

        MyCamera = GetComponent<Camera>();
        MyCamera.rect = new Rect(0f, 0f, 1f, 1f);
        // create RenderTexture and assign it to the main camera
        m_RenderTexture = MyCamera.targetTexture; // get it instead of create it
        MyCamera.pixelRect = new Rect(0, 0, m_RenderTexture.width, m_RenderTexture.height);
        MyCamera.Render();

        tex = new Texture2D(MyCamera.pixelWidth, MyCamera.pixelHeight, TextureFormat.BGRA32, false);
        Debug.Log("In start, camsize width:" + MyCamera.pixelWidth.ToString() + " height:" + MyCamera.pixelHeight.ToString());
    }

    void Update()
    {
        // Handle Request Queue
        while (requests.Count > 0)
        {
            // Get the first Request in the Queue
            AsyncGPUReadbackRequest request = requests.Peek();
            if (request.hasError)
            {
                // Error!
                Debug.LogWarning("AsyncGPUReadbackRequest Error! :(");
                requests.Dequeue(); // Remove from Queue
            }
            else if (request.done)
            {
                ReadbackData(request);
                requests.Dequeue(); // Remove from Queue
            }
            else
            {
                // Request is still processing.
                break;
            }
            // Note : We have to Dequeue items or break,
            // or we'll be caught in an infinite loop!
        }

        // Handle Request Timer
        timer += Time.deltaTime;
        if (timer > timeBetweenRequests)
        {
            timer = timer - timeBetweenRequests;
            AsyncGPUReadbackRequest rq = AsyncGPUReadback.Request(m_RenderTexture);
            requests.Enqueue(rq);
        }
    }

    void ReadbackData(AsyncGPUReadbackRequest request)
    {
        int len = request.GetData<Color32>().ToArray().Length;

        tex.SetPixels32(request.GetData<Color32>().ToArray());
        tex.Apply();

        sensor_msgs.msg.CompressedImage img = new sensor_msgs.msg.CompressedImage();
        img.Header = GetHeader();
        img.Format = "jpeg";
        img.Data = tex.EncodeToJPG(JpgQuality);

        if (ros2Unity.Ok())
        {
            chatter_pub.Publish(img);
        }
    }

    std_msgs.msg.Header GetHeader()
    {
        System.DateTime now = System.DateTime.UtcNow;

        std_msgs.msg.Header header = new std_msgs.msg.Header();
        builtin_interfaces.msg.Time time = new builtin_interfaces.msg.Time();
        time.Sec = now.Second;
        header.Stamp = time;
        header.Frame_id = frame_id;
        return header;
    }
}