linerobot/Unity/LineRobot/Assets/DepthCamera.cs

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

[RequireComponent(typeof(Camera))]
[ExecuteInEditMode]
public class DepthCamera : MonoBehaviour
{
    private Queue<AsyncGPUReadbackRequest> requests = new Queue<AsyncGPUReadbackRequest>();

    public float timeBetweenRequests = 0.2f;
    public string TopicName = "rt/depth_image";
    public string frame_id = null;
    public ComputeShader shader;

    private RenderTexture m_RenderTexture;
    private float timer = 0.0f;
    
    private ComputeBuffer buffer;
    private float[] output;
    private int kernel;

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

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

        Camera camera = GetComponent<Camera>();
        camera.depthTextureMode = camera.depthTextureMode | DepthTextureMode.Depth;

        m_RenderTexture = camera.targetTexture;
        m_RenderTexture.depth = 32;
        m_RenderTexture.enableRandomWrite = true; // ik wijzig texture niet dus niet nodig?
        camera.Render();

        Debug.Log("In depthcamera, depth: " + m_RenderTexture.depth.ToString());

        Debug.Log("In depthcamera, camsize width:" + m_RenderTexture.width.ToString() + " height:" + m_RenderTexture.height.ToString());

        buffer = new ComputeBuffer(m_RenderTexture.width * m_RenderTexture.height, sizeof(float));

        kernel = shader.FindKernel("CSMain");
        shader.SetTexture(kernel, "tex", m_RenderTexture);
        shader.SetBuffer(kernel, "output", buffer);
    }

    void Update()
    {
       
        // Handle Request Timer
        timer += Time.deltaTime;
        if (timer > timeBetweenRequests)
        {
            shader.Dispatch(kernel, m_RenderTexture.width/8, m_RenderTexture.height/8, 1);

            timer = timer - timeBetweenRequests;

            sensor_msgs.msg.Image img = CreateImageWithoutData();
            Debug.Log(img);
            img.Data = GetBytes(output);

            if (ros2Unity.Ok() && chatter_pub != null)
            {
                chatter_pub.Publish(img);
            }
        }
    }

    /*void ReadbackData(AsyncGPUReadbackRequest request)
    {

        var input = request.GetData<float>().ToArray();  //float is 32 bits

        //Debug.Log("Got a AsyncGPUReadbackRequest with size " + input.Length.ToString() + " and require size " + (MyCamera.pixelWidth * MyCamera.pixelHeight).ToString() + ", discarding it");

        sensor_msgs.msg.Image img = CreateImageWithoutData();
        img.Data = GetBytes(input);

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

    private byte[] GetBytes(float[] input)
    {
        byte[] messageinput = new byte[input.Length * 4];
        for (int i = 0; i < input.Length; i++)
        {
            byte[] newbytes = BitConverter.GetBytes(input[i]);
            for (int j = 0; j < newbytes.Length; j++)
            {
                messageinput[i * 4 + j] = newbytes[j];
            }
        }
        return messageinput;
    }

    private sensor_msgs.msg.Image CreateImageWithoutData()
    {
        sensor_msgs.msg.Image img = new sensor_msgs.msg.Image();

        // Image Header
        std_msgs.msg.Header header = new std_msgs.msg.Header();
        builtin_interfaces.msg.Time time = new builtin_interfaces.msg.Time();
        System.DateTime now = System.DateTime.UtcNow;
        time.Sec = now.Second;
        header.Stamp = time;
        header.Frame_id = frame_id;

        // Image Details
        img.Height = (uint) m_RenderTexture.height;
        img.Width = (uint) m_RenderTexture.width;
        img.Encoding = "32FC1";
        img.Is_bigendian = 0;
        img.Step = img.Width;

        return img;
    }

    private void OnDestroy()
    {
        //buffer.Dispose();
    }
}