Ask Your Question

Revision history [back]

click to hide/show revision 1
initial version

I know is a mess of code, but at least it subscribe to a pointclou2 and publishes a mesh. The only missing thing is to connect the [xyz] with the mesh. Do not know how to do it :)

using System.Collections;
using System.Collections.Generic;
using UnityEngine;
using System;
using RosSharp.RosBridgeClient;
using std_msgs = RosSharp.RosBridgeClient.MessageTypes.Std;
using sensor_msgs = RosSharp.RosBridgeClient.MessageTypes.Sensor;
using RosSharp.RosBridgeClient.Protocols;



[RequireComponent(typeof (RosConnector))]
//[RequireComponent(typeof (MeshFilter))]
public class PointCloudSubscriber : MonoBehaviour
{

    public string uri = "ws://10.42.0.1:9090";
    private RosSocket rosSocket;
    string subscriptionId = "";

    public RgbPoint3[] Points;

    //Mesh components
    Mesh mesh;
    Vector3[] vertices;
    int[] triangles;

    public int xSize = 20;
    public int zSize = 20;

    //Vector3[] newverts;



    // Start is called before the first frame update
    void Start()
    {

        mesh = new Mesh(); 
        GetComponent<MeshFilter>().mesh = mesh;

        CreateShape();
        UpdateMesh();

        //mesh.vertices = newVertices;
        //mesh.uv = newUV;
        //mesh.triangles = newTriangles;


            Debug.Log("RosSocket Initialization!!!");
        //RosSocket rosSocket = new RosSocket("ws://147.229.14.150:9090");
        rosSocket = new RosSocket(new  RosSharp.RosBridgeClient.Protocols.WebSocketNetProtocol(uri)); // 10.189.42.225:9090
        //Subscribe("/cloud");
        Subscribe("/zed/rtabmap/cloud_map");


    }



    void Update()
    {
        ////string publication_id = rosSocket.Advertise<std_msgs.String>("/talker");

        ////std_msgs.String message = new std_msgs.String
        ////{
        ////    data = "hello"
        ////};
        //RosSocket.Publish(publication_id, message);
        //RosSocket.Unadvertise(publication_id);
        //Thread.Sleep(100);
        //Assert.IsTrue(true);



        ////rosSocket.Publish(publication_id, message);

        //Subscribe("/zed/rtabmap/cloud_map");

    }

    void CreateShape(){

        vertices = new Vector3[(xSize+1)*(zSize+1)];

        for(int i = 0,  z =0; z <= zSize; z++)
        {
            for(int x =0; x <= xSize; x++){
                vertices[i] = new Vector3(x,0,z);
                i++;
            }
        }

    }

    void UpdateMesh()
    {
        mesh.Clear();
        mesh.vertices = vertices;
    }

    private void OnDrawGizmos()
    {
        if(vertices == null)
            return;
        for(int i =0; i < vertices.Length; i++)
        {
            Gizmos.DrawSphere(vertices[i], .1f);
        }
    }

    public void Subscribe(string id)
    {
        //subscriptionId  = rosSocket.Subscribe<std_msgs.String>(id, SubscriptionHandler);
                subscriptionId  = rosSocket.Subscribe<sensor_msgs.PointCloud2>(id, SubscriptionHandler);
        //StartCoroutine(WaitForKey());    

        // Subscription:
        //subscription_id = rosSocket.Subscribe<std_msgs.String>("/subscription_test", SubscriptionHandler);
        //subscription_id = rosSocket.Subscribe<std_msgs.String>("/subscription_test", SubscriptionHandler);

    }

    private IEnumerator WaitForKey()
    {
        Debug.Log("Press any key to close...");

        while (!Input.anyKeyDown)
        {
            yield return null;
        }

        Debug.Log("Closed");
        // rosSocket.Close();
    }

    private static void SubscriptionHandler(sensor_msgs.PointCloud2 message)
    {

        Debug.Log("Message received!");
        long I = message.data.Length / message.point_step;
        Debug.Log("Long I   " + I);
        RgbPoint3[] Points = new RgbPoint3[I];
        byte[] byteSlice = new byte[message.point_step];

        for (long i = 0; i < I; i++)
        {
            Array.Copy(message.data, i * message.point_step, byteSlice, 0, message.point_step);
            Points[i] = new RgbPoint3(byteSlice, message.fields);

            //Debug.Log("X   " + Points[i].x);
            //Debug.Log("Y   " + Points[i].y);
            //Debug.Log("Z   " + Points[i].z);


        }


        Vector3[] newverts = new Vector3[I];
        //Debug.Log("newvertes_X   " + newverts[0].x);

        for (var i = 0; i < I; i++)
        {

            newverts[i].x = Points[i].x;
            newverts[i].y = Points[i].y;
            newverts[i].z = Points[i].z;
            //Debug.Log("newvertes_X   " + newverts[i].x    );

        }

        //Mesh mesh = new Mesh();

        //GameObject  gameObject = new GameObject("Mesh", typeof(MeshFilter), typeof(MeshRenderer));

        //gameObject.GetComponent<MeshFilter>().mesh = mesh;

        //Mesh meshPC = new Mesh();

        //MeshFilter meshfilter = gameObject.AddComponent<MeshFilter>();

        //meshPC = GetComponent<MeshFilter>().meshPC;
        //meshPC.Clear();

    }
}

I know is a mess of code, but at least it subscribe to a pointclou2 and publishes a mesh. The only missing thing is to connect the [xyz] with the mesh. Do not know how connect to do it make a proper transformation from ros 2 unity :)

using System.Collections; using System.Collections.Generic; using UnityEngine; using System; using RosSharp.RosBridgeClient; using std_msgs = RosSharp.RosBridgeClient.MessageTypes.Std; using sensor_msgs = RosSharp.RosBridgeClient.MessageTypes.Sensor; using RosSharp.RosBridgeClient.Protocols; RosSharp.RosBridgeClient.Protocols;

using System.IO;

using Object = UnityEngine.Object;

[RequireComponent(typeof (RosConnector))] //[RequireComponent(typeof [RequireComponent(typeof (MeshFilter))] public class PointCloudSubscriber : MonoBehaviour { {

public string uri = "ws://10.42.0.1:9090";
 private RosSocket rosSocket;
 string subscriptionId = "";

 public RgbPoint3[] Points;

 //Mesh components
 private Mesh mesh;
 Vector3[] vertices;
 int[] triangles;

 public int xSize = 20;
 public int zSize = 20;

 //Vector3[] newverts;

 private bool update_mesh = false;


// Start is called before the first frame update
 void Start()
    {

    {

    mesh = new Mesh(); 
     GetComponent<MeshFilter>().mesh = mesh;

        CreateShape();
        UpdateMesh();

    //CreateShape();
    //UpdateMesh();

    //mesh.vertices = newVertices;
     //mesh.uv = newUV;
     //mesh.triangles = newTriangles;


         Debug.Log("RosSocket Initialization!!!");
     //RosSocket rosSocket = new RosSocket("ws://147.229.14.150:9090");
     rosSocket = new RosSocket(new  RosSharp.RosBridgeClient.Protocols.WebSocketNetProtocol(uri)); // 10.189.42.225:9090
     //Subscribe("/cloud");
     Subscribe("/zed/rtabmap/cloud_map");


    }



    }



void Update()
    {
    {


    ////string publication_id = rosSocket.Advertise<std_msgs.String>("/talker");

     ////std_msgs.String message = new std_msgs.String
     ////{
     ////    data = "hello"
     ////};
     //RosSocket.Publish(publication_id, message);
     //RosSocket.Unadvertise(publication_id);
     //Thread.Sleep(100);
     //Assert.IsTrue(true);



     ////rosSocket.Publish(publication_id, message);

     //Subscribe("/zed/rtabmap/cloud_map");

    }

    if(update_mesh) {
        update_mesh = false;

        //CreateShape();
        UpdateMesh();
    }

}



void CreateShape(){
     Debug.Log("received one");
    vertices = new Vector3[(xSize+1)*(zSize+1)];

     for(int i = 0,  z =0; z <= zSize; z++)
        {
    {
        for(int x =0; x <= xSize; x++){
             vertices[i] = new Vector3(x,0,z);
             i++;
            }
        }

    }

    }
    }
    Debug.Log("received two");
}

void UpdateMesh()
 {
    Debug.Log("received three");

    try {
        mesh.Clear();
    } catch (Exception e) {
        Debug.Log(e);
    }
    Debug.Log("received four");

    mesh.vertices = vertices;
    }

    private 

}

public void OnDrawGizmos()
    {
    {
    Gizmos.color = new Color(1, 0.2F, 0, 0.5F);
    if(vertices == null)
         return;
     for(int i =0; i < vertices.Length; i++)
        {
    {
        //Gizmos.DrawSphere(vertices[i], .1f);
        Gizmos.DrawCube(vertices[i], new Vector3(0.1f, 0.1f, 0.1f));
    }

}   

/*public void OnDrawGizmos()
{
    if(vertices == null)
        return;
    for(int i =0; i < vertices.Length; i++)
    {
        Gizmos.DrawSphere(vertices[i], .1f);
        }
    }

    //Gizmos.DrawWireCube(vertices[i], .1f);
    }
}*/

public void Subscribe(string id)
    {
    {
    //subscriptionId  = rosSocket.Subscribe<std_msgs.String>(id, SubscriptionHandler);
             subscriptionId  = rosSocket.Subscribe<sensor_msgs.PointCloud2>(id, SubscriptionHandler);
     //StartCoroutine(WaitForKey());    

     // Subscription:
     //subscription_id = rosSocket.Subscribe<std_msgs.String>("/subscription_test", SubscriptionHandler);
     //subscription_id = rosSocket.Subscribe<std_msgs.String>("/subscription_test", SubscriptionHandler);

    }

    }

private IEnumerator WaitForKey()
    {
    {
    Debug.Log("Press any key to close...");

     while (!Input.anyKeyDown)
        {
    {
        yield return null;
        }

    }

    Debug.Log("Closed");
     // rosSocket.Close();
    }

    private static }

public void SubscriptionHandler(sensor_msgs.PointCloud2 message)
    {

    {

    Debug.Log("Message received!");
     long I = message.data.Length / message.point_step;
     Debug.Log("Long I   " + I);
     RgbPoint3[] Points = new RgbPoint3[I];
     byte[] byteSlice = new byte[message.point_step];

     for (long i = 0; i < I; i++)
        {
    {
        Array.Copy(message.data, i * message.point_step, byteSlice, 0, message.point_step);
         Points[i] = new RgbPoint3(byteSlice, message.fields);

         //Debug.Log("X   " + Points[i].x);
         //Debug.Log("Y   " + Points[i].y);
         //Debug.Log("Z   " + Points[i].z);


        }


        Vector3[] }


    //Vector3[] newverts = new Vector3[I];
    vertices = new Vector3[I];
    //Debug.Log("newvertes_X   " + newverts[0].x);

     for (var i = 0; i < I; i++)
        {

            newverts[i].x {

        //newverts[i].x = Points[i].x;
            newverts[i].y //newverts[i].y = Points[i].y;
            newverts[i].z //newverts[i].z = Points[i].z;
        vertices[i].x = Points[i].x;
        vertices[i].y = Points[i].y;
        vertices[i].z = Points[i].z;
        //Debug.Log("newvertes_X   " + newverts[i].x    );

        }

    }

    //vertices.Ros2Unity();

    //CreateShape();
    //UpdateMesh();

    update_mesh = true;







    //Mesh mesh = new Mesh();
     //GetComponent<MeshFilter>().mesh = mesh;

    //mesh.Clear();
    //mesh.vertices = newverts;


    //GameObject  gameObject = new GameObject("Mesh", typeof(MeshFilter), typeof(MeshRenderer));

     //gameObject.GetComponent<MeshFilter>().mesh = mesh;

     //Mesh meshPC = new Mesh();

     //MeshFilter meshfilter = gameObject.AddComponent<MeshFilter>();

     //meshPC = GetComponent<MeshFilter>().meshPC;
     //meshPC.Clear();

    }
}

}

I know is a mess of code, but at least it subscribe to a pointclou2 and publishes a mesh. The only missing thing is mesh and also transform from ROS to connect to make a proper transformation from ros 2 unity :)coordinate systems.

using

  ´╗┐using System.Collections;
using System.Collections.Generic;
using UnityEngine;
using System;
using RosSharp.RosBridgeClient;
using std_msgs = RosSharp.RosBridgeClient.MessageTypes.Std;
using sensor_msgs = RosSharp.RosBridgeClient.MessageTypes.Sensor;
//using RosSharp.RosBridgeClient.Protocols;


using RosSharp.RosBridgeClient.Protocols;

using System.IO;

System.IO; using Object = UnityEngine.Object;

UnityEngine.Object; [RequireComponent(typeof (RosConnector))] [RequireComponent(typeof (MeshFilter))] public class PointCloudSubscriber : MonoBehaviour {

{

    public string uri = "ws://10.42.0.1:9090";
 private RosSocket rosSocket;
 string subscriptionId = "";

 public RgbPoint3[] Points;

 //Mesh components
 private Mesh mesh;
 Vector3[] vertices;
 int[] triangles;

 public int xSize = 20;
 public int zSize = 20;

 //Vector3[] newverts;

 private bool update_mesh = false;


 // Start is called before the first frame update
 void Start()
 {

     mesh = new Mesh(); 
     GetComponent<MeshFilter>().mesh = mesh;

     //CreateShape();
     //UpdateMesh();

     //mesh.vertices = newVertices;
     //mesh.uv = newUV;
     //mesh.triangles = newTriangles;


         Debug.Log("RosSocket Initialization!!!");
     //RosSocket rosSocket = new RosSocket("ws://147.229.14.150:9090");
     rosSocket = new RosSocket(new  RosSharp.RosBridgeClient.Protocols.WebSocketNetProtocol(uri)); // 10.189.42.225:9090
     //Subscribe("/cloud");
    Subscribe("/zed/rtabmap/cloud_map");


    //Subscribe("/zed/rtabmap/cloud_map");
        Subscribe("/octomap_point_cloud_centers");


    }



 void Update()
 {


     ////string publication_id = rosSocket.Advertise<std_msgs.String>("/talker");

     ////std_msgs.String message = new std_msgs.String
     ////{
     ////    data = "hello"
     ////};
     //RosSocket.Publish(publication_id, message);
     //RosSocket.Unadvertise(publication_id);
     //Thread.Sleep(100);
     //Assert.IsTrue(true);



     ////rosSocket.Publish(publication_id, message);

     //Subscribe("/zed/rtabmap/cloud_map");

     if(update_mesh) {
         update_mesh = false;

         //CreateShape();
         UpdateMesh();
     }

 }



 void CreateShape(){
     Debug.Log("received one");
     vertices = new Vector3[(xSize+1)*(zSize+1)];

     for(int i = 0,  z =0; z <= zSize; z++)
     {
         for(int x =0; x <= xSize; x++){
             vertices[i] = new Vector3(x,0,z);
             i++;
         }
     }
     Debug.Log("received two");
 }

 void UpdateMesh()
 {
     Debug.Log("received three");

     try {
         mesh.Clear();
     } catch (Exception e) {
         Debug.Log(e);
     }
     Debug.Log("received four");

     mesh.vertices = vertices;


 }

 public void OnDrawGizmos()
 {
     Gizmos.color = new Color(1, 0.2F, 0, 0.5F);
     if(vertices == null)
         return;
     for(int i =0; i < vertices.Length; i++)
     {
         //Gizmos.DrawSphere(vertices[i], .1f);
         Gizmos.DrawCube(vertices[i], new Vector3(0.1f, 0.1f, 0.1f));
     }

 }   

 /*public void OnDrawGizmos()
 {
     if(vertices == null)
         return;
     for(int i =0; i < vertices.Length; i++)
     {
         Gizmos.DrawSphere(vertices[i], .1f);
         //Gizmos.DrawWireCube(vertices[i], .1f);
     }
 }*/

 public void Subscribe(string id)
 {
     //subscriptionId  = rosSocket.Subscribe<std_msgs.String>(id, SubscriptionHandler);
             subscriptionId  = rosSocket.Subscribe<sensor_msgs.PointCloud2>(id, SubscriptionHandler);
     //StartCoroutine(WaitForKey());    

     // Subscription:
     //subscription_id = rosSocket.Subscribe<std_msgs.String>("/subscription_test", SubscriptionHandler);
     //subscription_id = rosSocket.Subscribe<std_msgs.String>("/subscription_test", SubscriptionHandler);

 }

 private IEnumerator WaitForKey()
 {
     Debug.Log("Press any key to close...");

     while (!Input.anyKeyDown)
     {
         yield return null;
     }

     Debug.Log("Closed");
     // rosSocket.Close();
 }

 public void SubscriptionHandler(sensor_msgs.PointCloud2 message)
 {

     Debug.Log("Message received!");
     long I = message.data.Length / message.point_step;
     Debug.Log("Long I   " + I);
     RgbPoint3[] Points = new RgbPoint3[I];
     byte[] byteSlice = new byte[message.point_step];

     for (long i = 0; i < I; i++)
     {
         Array.Copy(message.data, i * message.point_step, byteSlice, 0, message.point_step);
         Points[i] = new RgbPoint3(byteSlice, message.fields);

         //Debug.Log("X   " + Points[i].x);
         //Debug.Log("Y   " + Points[i].y);
         //Debug.Log("Z   " + Points[i].z);


     }


     //Vector3[] newverts = new Vector3[I];
     vertices = new Vector3[I];
     //Debug.Log("newvertes_X   " + newverts[0].x);

     for (var i = 0; i < I; i++)
     {

         //newverts[i].x = Points[i].x;
         //newverts[i].y = Points[i].y;
         //newverts[i].z = Points[i].z;
         vertices[i].x = Points[i].x;
         vertices[i].y = Points[i].z;
            vertices[i].z = Points[i].y;
        vertices[i].z = Points[i].z;
        //Debug.Log("newvertes_X   " + newverts[i].x    );

     }


    //for (var i = 0; i < I; i++)
    //  {


    //      vertices[i] = vertices(vertices[i].x,vertices[i].y,vertices[i].z).Ros2Unity();


    //  }

        //vertices.Ros2Unity();

     //CreateShape();
     //UpdateMesh();

     update_mesh = true;







     //Mesh mesh = new Mesh();
     //GetComponent<MeshFilter>().mesh = mesh;

     //mesh.Clear();
     //mesh.vertices = newverts;


     //GameObject  gameObject = new GameObject("Mesh", typeof(MeshFilter), typeof(MeshRenderer));

     //gameObject.GetComponent<MeshFilter>().mesh = mesh;

     //Mesh meshPC = new Mesh();

     //MeshFilter meshfilter = gameObject.AddComponent<MeshFilter>();

     //meshPC = GetComponent<MeshFilter>().meshPC;
     //meshPC.Clear();

 }

}