linerobot/Unity/LineRobot/Assets/DepthPostprocessor.cs

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

[RequireComponent(typeof(Camera))]
public class DepthPostprocessor : MonoBehaviour
{

    public float timeBetweenRequests = 0.2f;
    public string TopicName = "rt/depth_processor";
    public string frame_id = null;

    //material that's applied when doing postprocessing
    [SerializeField]
    private Material mater;

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

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

    private void Start()
    {
        ros2Unity = GetComponent<ROS2UnityComponent>();
        if (ros2Unity.Ok())
        {
            if (ros2Node == null)
            {
                ros2Node = ros2Unity.CreateNode("ROS2LFRdepthprocessor");
            }
            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?

        buffer = new ComputeBuffer(m_RenderTexture.width * m_RenderTexture.height, sizeof(float));
        output = new float[m_RenderTexture.width * m_RenderTexture.height];
        output[0] = 0.2f;
        output[30000] = 0.212f;
        buffer.SetData(output); // nodig?
        Graphics.SetRandomWriteTarget(1, buffer, true); // 1?

        mater.SetInt("_width", m_RenderTexture.width);
        mater.SetInt("_height", m_RenderTexture.height);
        mater.SetBuffer("_myBuffer", buffer);
    }

    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 * 4; // 4 want float is 4 bytes

        return img;
    }

    private byte[] GetBytes(float[] input)
    {
        byte[] messageinput = new byte[input.Length * 4];
        for (int i = 0; i < input.Length; i++)
        {
            byte[] newbytes = System.BitConverter.GetBytes(input[i]);
            for (int j = 0; j < newbytes.Length; j++)
            {
                messageinput[i * 4 + j] = newbytes[j];
            }
        }
        return messageinput;
    }
    //method which is automatically called by unity after the camera is done rendering
    void OnRenderImage(RenderTexture source, RenderTexture destination)
    {
        Graphics.Blit(source, destination, mater);

        timer += Time.deltaTime;
        if (timer > timeBetweenRequests)
        {
            //draws the pixels from the source texture to the destination texture

            buffer.GetData(output);

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

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

            timer = timer - timeBetweenRequests;
        }
    }
    private void OnDestroy()
    {
        buffer.Dispose();
    }
}