public void ReceiveMessage(RosSharp.RosBridgeClient.Messages.Sensor.PointCloud2 message) { int oneAxisValueByteSize = 4; // 4(float32) int oneVertexByteSize = message.point_step; int numberOfVertices = message.data.Length / oneVertexByteSize; Vector3[] newPointCloudVertices = new Vector3[numberOfVertices]; if (message.data != null) { int offset = 0; for (int i = 0; i < numberOfVertices; i++) { float x = BitConverter.ToSingle(message.data, offset); float y = BitConverter.ToSingle(message.data, offset + oneAxisValueByteSize); float z = BitConverter.ToSingle(message.data, offset + 2 * oneAxisValueByteSize); Vector3 pointInROSCoordinates = new Vector3(x, y, z); Vector3 pointInUnityCoordinates = pointInROSCoordinates.Ros2Unity(); newPointCloudVertices[i] = pointInUnityCoordinates; offset = offset + oneVertexByteSize; } System.Diagnostics.Debug.WriteLine("Enqueued a new point cloud with " + newPointCloudVertices.Length.ToString() + " points"); internalQueue.Enqueue(newPointCloudVertices); } }
public void SubscriptionHandler(sensor_msgs.PointCloud2 message) { NumberOfClouds = message.data.Length / message.point_step; // Cut the pointcloud to points if (NumberOfClouds > layer * MaxVerts) { NumberToGenerate = (NumberOfClouds - layer * MaxVerts) / MaxVerts + 1; layer += NumberToGenerate; createGO = true; } RgbPoint3[] Points = new RgbPoint3[NumberOfClouds]; byte[] byteSlice = new byte[message.point_step]; for (long i = 0; i < NumberOfClouds; i++) { Array.Copy(message.data, i * message.point_step, byteSlice, 0, message.point_step); Points[i] = new RgbPoint3(byteSlice, message.fields); } lastMessage = Points; vertices2d = new Vector3[layer][]; triangles2d = new int[layer][]; colors2d = new Color[layer][]; SetupMeshArrays(Points); updateMesh = true; }
private void SendPointCloud(TimeSpan currentTime, List <Matrix4x4> transforms, List <Vector3[]> vertices) { int oneAxisValueByteSize = 4; // 4(float32) int oneVertexByteSize = 3 * oneAxisValueByteSize; int numberOfVertices = vertices.Sum(group => group.Length); byte[] data = new byte[numberOfVertices * oneVertexByteSize]; int ngroups = transforms.Count; int copiedUntilNow = 0; for (int i = 0; i < ngroups; i++) { Matrix4x4 currentTransfrom = transforms[i]; Vector3[] currentVertices = vertices[i]; int subNumberOfVertices = currentVertices.Length; for (int j = 0; j < subNumberOfVertices; j++) { Vector3 vertex = currentVertices[j]; Vector3 transformedVertex = currentTransfrom.MultiplyPoint3x4(vertex); transformedVertex = HololensRobotController.Utilities.CoordinateTransformations.ConvertPositionUnity2ROS(transformedVertex); Buffer.BlockCopy(BitConverter.GetBytes(transformedVertex.x), 0, data, copiedUntilNow + j * oneVertexByteSize, oneAxisValueByteSize); Buffer.BlockCopy(BitConverter.GetBytes(transformedVertex.y), 0, data, copiedUntilNow + j * oneVertexByteSize + oneAxisValueByteSize, oneAxisValueByteSize); Buffer.BlockCopy(BitConverter.GetBytes(transformedVertex.z), 0, data, copiedUntilNow + j * oneVertexByteSize + 2 * oneAxisValueByteSize, oneAxisValueByteSize); } copiedUntilNow = copiedUntilNow + subNumberOfVertices * oneVertexByteSize; } // pulish the message int[] structuredTime = Timer.GetSecondsNanosecondsStructure(currentTime); RosSharp.RosBridgeClient.Messages.Sensor.PointCloud2 message = new RosSharp.RosBridgeClient.Messages.Sensor.PointCloud2(); message.header.frame_id = Config.HololensWorldFrame; message.header.seq = frameIdx++; message.header.stamp.secs = structuredTime[0]; message.header.stamp.nsecs = structuredTime[1]; message.height = 1; message.width = numberOfVertices; message.fields = pointFields; message.is_bigendian = false; message.is_dense = true; message.point_step = oneVertexByteSize; message.row_step = message.width * message.point_step; message.data = data; publisher.Publish(message); }
public void SubscriptionHandler(sensor_msgs.PointCloud2 message) { long I = message.data.Length / message.point_step; 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); } newverts = new Vector3[I]; double[] y_array = new double[I]; //Assign all PointCloud points to the Vecto3[] for (var i = 0; i < I; i++) { newverts[i] = new Vector3(Points[i].x, Points[i].z, Points[i].y); y_array[i] = newverts[i].y; } if (I < Ik) { flag_object_init = true; } if (I > ((layer + 1) * Ik)) { if (flag == true) { layer++; flag = false; } if (I > (layer * Ik)) { Ic = (int)Iold; //layer*Ik; flag = true; flag_object = true; create_mesh = false; objcount++; } } CreateMesh(I, Ic, newverts); Iold = I; }
public void SubscriptionHandler(sensor_msgs.PointCloud2 message) { 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); } newverts = new Vector3[I]; double[] y_array = new double[I]; //Assign all PointCloud points to the Vecto3[] for (var i = 0; i < I; i++) { newverts[i] = new Vector3(Points[i].x, Points[i].z, Points[i].y); y_array[i] = newverts[i].y; VectorList.Add(newverts[i]); } float inc = 0.15f; //Assign all the vertices vertices = new Vector3[I * 8]; int vinc = 0; for (int k = 0; k < I; k++) { for (int i = 0; i < I * 8; i++) { if (i == 0 + vinc) { vertices[i] = new Vector3(newverts[k].x - inc, newverts[k].y - inc, newverts[k].z + inc); } else if (i == 1 + vinc) { vertices[i] = new Vector3(newverts[k].x - inc, newverts[k].y - inc, newverts[k].z - inc); } else if (i == 2 + vinc) { vertices[i] = new Vector3(newverts[k].x - inc, newverts[k].y + inc, newverts[k].z - inc); } else if (i == 3 + vinc) { vertices[i] = new Vector3(newverts[k].x - inc, newverts[k].y + inc, newverts[k].z + inc); } else if (i == 4 + vinc) { vertices[i] = new Vector3(newverts[k].x + inc, newverts[k].y + inc, newverts[k].z + inc); } else if (i == 5 + vinc) { vertices[i] = new Vector3(newverts[k].x + inc, newverts[k].y + inc, newverts[k].z - inc); } else if (i == 6 + vinc) { vertices[i] = new Vector3(newverts[k].x + inc, newverts[k].y - inc, newverts[k].z - inc); } else if (i == 7 + vinc) { vertices[i] = new Vector3(newverts[k].x + inc, newverts[k].y - inc, newverts[k].z + inc); } } vinc = vinc + 8; } int tris = 0; //36; int vert = 0; //8; triangles = new int[36 * I]; //Assign triangules of all the vertices for (int k = 0; k < I; k++) { for (int i = 0; i < 36; i++) { if (i == 0) { triangles[i + tris] = 0 + vert; } else if (i == 1) { triangles[i + tris] = 2 + vert; } else if (i == 2) { triangles[i + tris] = 1 + vert; } else if (i == 3) { triangles[i + tris] = 0 + vert; } else if (i == 4) { triangles[i + tris] = 3 + vert; } else if (i == 5) { triangles[i + tris] = 2 + vert; } else if (i == 6) { triangles[i + tris] = 2 + vert; } else if (i == 7) { triangles[i + tris] = 3 + vert; } else if (i == 8) { triangles[i + tris] = 4 + vert; } else if (i == 9) { triangles[i + tris] = 2 + vert; } else if (i == 10) { triangles[i + tris] = 4 + vert; } else if (i == 11) { triangles[i + tris] = 5 + vert; } else if (i == 12) { triangles[i + tris] = 1 + vert; } else if (i == 13) { triangles[i + tris] = 2 + vert; } else if (i == 14) { triangles[i + tris] = 5 + vert; } else if (i == 15) { triangles[i + tris] = 1 + vert; } else if (i == 16) { triangles[i + tris] = 5 + vert; } else if (i == 17) { triangles[i + tris] = 6 + vert; } else if (i == 18) { triangles[i + tris] = 0 + vert; } else if (i == 19) { triangles[i + tris] = 7 + vert; } else if (i == 20) { triangles[i + tris] = 4 + vert; } else if (i == 21) { triangles[i + tris] = 0 + vert; } else if (i == 22) { triangles[i + tris] = 4 + vert; } else if (i == 23) { triangles[i + tris] = 3 + vert; } else if (i == 24) { triangles[i + tris] = 5 + vert; } else if (i == 25) { triangles[i + tris] = 4 + vert; } else if (i == 26) { triangles[i + tris] = 7 + vert; } else if (i == 27) { triangles[i + tris] = 5 + vert; } else if (i == 28) { triangles[i + tris] = 7 + vert; } else if (i == 29) { triangles[i + tris] = 6 + vert; } else if (i == 30) { triangles[i + tris] = 0 + vert; } else if (i == 31) { triangles[i + tris] = 6 + vert; } else if (i == 32) { triangles[i + tris] = 7 + vert; } else if (i == 33) { triangles[i + tris] = 0 + vert; } else if (i == 34) { triangles[i + tris] = 1 + vert; } else if (i == 35) { triangles[i + tris] = 6 + vert; } } vert += 8; tris += 36; } colors = new Color[vertices.Length]; index = 0; ci = 0; //Arrange array [y] with no repeat values List <double> unique_y = new List <double>(); for (int i = 0; i < y_array.Length; i++) { bool found = false; for (int prev = 0; prev < i; prev++) { if (y_array[prev] == y_array[i]) { found = true; break; } } if (!found) { //unique_y.Add(Math.Round(y_array[i],2)); unique_y.Add(y_array[i]); } } //Debug.Log("Y_ARRAY " + y_array.Length); y_array = unique_y.ToArray(); //Color the octree cubes for (int i = 0; i < y_array.Length; i++) { for (int k = 0; k < I; k++) { if (y_array[i] == newverts[k].y) { ColorLayer(k, index); } } index++; if (index == 6) { index = 0; } } update_mesh = true; }
public void SubscriptionHandler(sensor_msgs.PointCloud2 message) { long NumberOfClouds = message.data.Length / message.point_step; // Cut the pointcloud to points Debug.Log(NumberOfClouds); if (NumberOfClouds > layer * MaxVerts) { layer++; createGO = true; } RgbPoint3[] Points = new RgbPoint3[NumberOfClouds]; byte[] byteSlice = new byte[message.point_step]; for (long i = 0; i < NumberOfClouds; i++) { Array.Copy(message.data, i * message.point_step, byteSlice, 0, message.point_step); Points[i] = new RgbPoint3(byteSlice, message.fields); } newverts = new Vector3[NumberOfClouds]; //Assign all PointCloud points to the Vector3[] for (var i = 0; i < NumberOfClouds; i++) { newverts[i] = new Vector3(Points[i].x, Points[i].z, Points[i].y); } vertices2d = new Vector3[layer][]; triangles2d = new int[layer][]; colors2d = new Color[layer][]; long arraySize; for (int l = 0; l < layer; l++) { if (l + 1 == layer) { arraySize = -(layer - 1) * MaxVerts + NumberOfClouds; } else { arraySize = MaxVerts; } float inc = 0.3f; int vinc = 0; int tris = 0; int vert = 0; int first = l * MaxVerts; int last = l * (int)MaxVerts + (int)arraySize; vertices2d[l] = new Vector3[arraySize * 8]; triangles2d[l] = new int[36 * (last - first)]; colors2d[l] = new Color[arraySize * 8]; for (int k = first; k < last; k++) { for (int i = 0; i < (last - first) * 8; i++) { if (i == 0 + vinc) { vertices2d[l][i] = new Vector3(newverts[k].x - inc, newverts[k].y - inc, newverts[k].z + inc); } else if (i == 1 + vinc) { vertices2d[l][i] = new Vector3(newverts[k].x - inc, newverts[k].y - inc, newverts[k].z - inc); } else if (i == 2 + vinc) { vertices2d[l][i] = new Vector3(newverts[k].x - inc, newverts[k].y + inc, newverts[k].z - inc); } else if (i == 3 + vinc) { vertices2d[l][i] = new Vector3(newverts[k].x - inc, newverts[k].y + inc, newverts[k].z + inc); } else if (i == 4 + vinc) { vertices2d[l][i] = new Vector3(newverts[k].x + inc, newverts[k].y + inc, newverts[k].z + inc); } else if (i == 5 + vinc) { vertices2d[l][i] = new Vector3(newverts[k].x + inc, newverts[k].y + inc, newverts[k].z - inc); } else if (i == 6 + vinc) { vertices2d[l][i] = new Vector3(newverts[k].x + inc, newverts[k].y - inc, newverts[k].z - inc); } else if (i == 7 + vinc) { vertices2d[l][i] = new Vector3(newverts[k].x + inc, newverts[k].y - inc, newverts[k].z + inc); } } vinc = vinc + 8; for (int i = 0; i < 36; i++) { if (i == 0) { triangles2d[l][i + tris] = 0 + vert; } else if (i == 1) { triangles2d[l][i + tris] = 2 + vert; } else if (i == 2) { triangles2d[l][i + tris] = 1 + vert; } else if (i == 3) { triangles2d[l][i + tris] = 0 + vert; } else if (i == 4) { triangles2d[l][i + tris] = 3 + vert; } else if (i == 5) { triangles2d[l][i + tris] = 2 + vert; } else if (i == 6) { triangles2d[l][i + tris] = 2 + vert; } else if (i == 7) { triangles2d[l][i + tris] = 3 + vert; } else if (i == 8) { triangles2d[l][i + tris] = 4 + vert; } else if (i == 9) { triangles2d[l][i + tris] = 2 + vert; } else if (i == 10) { triangles2d[l][i + tris] = 4 + vert; } else if (i == 11) { triangles2d[l][i + tris] = 5 + vert; } else if (i == 12) { triangles2d[l][i + tris] = 1 + vert; } else if (i == 13) { triangles2d[l][i + tris] = 2 + vert; } else if (i == 14) { triangles2d[l][i + tris] = 5 + vert; } else if (i == 15) { triangles2d[l][i + tris] = 1 + vert; } else if (i == 16) { triangles2d[l][i + tris] = 5 + vert; } else if (i == 17) { triangles2d[l][i + tris] = 6 + vert; } else if (i == 18) { triangles2d[l][i + tris] = 0 + vert; } else if (i == 19) { triangles2d[l][i + tris] = 7 + vert; } else if (i == 20) { triangles2d[l][i + tris] = 4 + vert; } else if (i == 21) { triangles2d[l][i + tris] = 0 + vert; } else if (i == 22) { triangles2d[l][i + tris] = 4 + vert; } else if (i == 23) { triangles2d[l][i + tris] = 3 + vert; } else if (i == 24) { triangles2d[l][i + tris] = 5 + vert; } else if (i == 25) { triangles2d[l][i + tris] = 4 + vert; } else if (i == 26) { triangles2d[l][i + tris] = 7 + vert; } else if (i == 27) { triangles2d[l][i + tris] = 5 + vert; } else if (i == 28) { triangles2d[l][i + tris] = 7 + vert; } else if (i == 29) { triangles2d[l][i + tris] = 6 + vert; } else if (i == 30) { triangles2d[l][i + tris] = 0 + vert; } else if (i == 31) { triangles2d[l][i + tris] = 6 + vert; } else if (i == 32) { triangles2d[l][i + tris] = 7 + vert; } else if (i == 33) { triangles2d[l][i + tris] = 0 + vert; } else if (i == 34) { triangles2d[l][i + tris] = 1 + vert; } else if (i == 35) { triangles2d[l][i + tris] = 6 + vert; } } vert += 8; tris += 36; for (int i = 0; i < 8; i++) { ci = i + (8 * (k - first)); colors2d[l][ci] = new Color32((byte)Points[k].rgb[0], (byte)Points[k].rgb[1], (byte)Points[k].rgb[2], 255); } } } updateMesh = true; }