ROS Sharp Unity3d import PointCloud2
Dear People,
I do have ros kinetic
and Ubuntu 16.04.5
and I have installed Unity3d-2018.3.0f2
Does any one has experience in importing a Poincloud2
from ROS
to Unity3d
.
Any advice I will appreciate it
Thank you very much :)
Asked by acp on 2019-12-09 06:29:24 UTC
Answers
I know is a mess of code, but at least it subscribe to a pointclou2 and publishes a mesh and also transform from ROS to unity coordinate systems.
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 System.IO;
using 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("/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;
//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();
}
Asked by acp on 2020-01-08 07:14:39 UTC
Comments
Hey there, I think I'm trying to do the same thing as you. Couple of questions:
What RgbPoint3 class are you using? I appear to have two: one from Pcx editor and one defined in PointCloud.cs in the ROS# repo.
Is there any particular reason you are setting up the ros socket yourself instead of adding rosconnector as a script?
I'm new to Unity, how are you transforming to a mesh? Are you essentially having to define your own?
Think you could post a screenshot of your implementation? That would help immensely.
Thanks for this.
Asked by brady_kruse on 2020-05-07 17:04:06 UTC
Hi,
Sorry for the late replay, I just saw your comment :)
I am using rossocket because I am communicating from a drone that runs ros.
I think I am using the one defined in PointCloud.cs in the ROS# repo.
well, the implementation is big for a screenshot :)
This is the link of the file https://www.dropbox.com/s/bsgihisl2r5fau3/PointCloudSubscriber_original.cs?dl=0
cheers
Asked by acp on 2020-08-06 02:03:46 UTC
Comments