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;
}
