/// <summary>
/// Update the mesh with the new mesh.
/// </summary>
/// <param name="meshMsg">ROSBridge PCFace Mesh Message</param>
public void SetMesh(PCFaceMsg meshMsg)
{
Debug.Log("Setting New PCFace Mesh");
List <Vector3> newVertices = new List <Vector3>();
// Also not sure what to do with all the newColors...
List <Color> newColors = new List <Color>();
List <int> newTriangles = new List <int>();
float[] x = meshMsg.Vert_x;
float[] y = meshMsg.Vert_y;
float[] z = meshMsg.Vert_z;
// byte[] r = meshMsg.Color_r;
// byte[] g = meshMsg.Color_g;
// byte[] b = meshMsg.Color_b;
// byte[] a = meshMsg.Color_a;
ushort[] face_0 = meshMsg.Face_0;
ushort[] face_1 = meshMsg.Face_1;
ushort[] face_2 = meshMsg.Face_2;
// Create a list of Vertices and their colors.
for (int j = 0; j < x.Length; j++)
{
if (flipYZ)
{
newVertices.Add(new Vector3(x[j], z[j], y[j]));
}
else
{
newVertices.Add(new Vector3(x[j], y[j], z[j]));
}
//newColors.Add(new Color(r[j], g[j], b[j], a[j]));
newColors.Add(Color.red);
}
Vector3[] vertices = newVertices.ToArray();
Color[] colors = newColors.ToArray();
// Create a list of mesh triangles from the faces arrays. There are as many faces as the length of the face array.
int[] triangles = new int[face_0.Length * 3];
for (int j = 0; j < face_0.Length; j++)
{
triangles[3 * j] = (int)face_0[j];
triangles[3 * j + 1] = (int)face_1[j];
triangles[3 * j + 2] = (int)face_2[j];
}
Debug.Log("Face_0 Length: " + meshMsg.Face_0.Length + "Face_1 Length: " + meshMsg.Face_1.Length + "Face_2 Length: " + meshMsg.Face_2.Length);
Debug.Log("Num Verticies: " + meshMsg.Vert_x.Length);
Debug.Log("Num Colors: " + meshMsg.Color_a.Length);
// Generate the Mesh
Mesh mesh = new Mesh();
mesh.vertices = vertices;
//mesh.uv = newUV;
mesh.triangles = triangles;
mesh.colors = colors;
meshParent.GetComponent <MeshFilter>().mesh = mesh;
hasChanged = true;
}
// ROS Topic Subscriber methods
public ROSBridgeMsg OnReceiveMessage(string topic, JSONNode raw_msg, ROSBridgeMsg parsed = null)
{
Debug.Log(" PC Face Mesh Recieved message");
ROSBridgeMsg result = null;
bool parsePCFaceMesh = false;
/// Color of the mesh
Color color = Color.white;
// Writing all code in here for now. May need to move out to separate handler functions when it gets too unwieldy.
switch (topic)
{
case "/colorized_points_faced_0":
Debug.Log("PC Face Mesh Visualizer Callback: " + topic);
parsePCFaceMesh = true;
color = new Color(0, 0, 0.5f, alpha);
break;
case "/colorized_points_faced_1":
Debug.Log("PC Face Mesh Visualizer Callback: " + topic);
parsePCFaceMesh = true;
color = new Color(0, 0, 1, alpha);
break;
case "/colorized_points_faced_2":
Debug.Log("PC Face Mesh Visualizer Callback: " + topic);
parsePCFaceMesh = true;
color = new Color(0, 1, 1, alpha);
break;
case "/colorized_points_faced_3":
Debug.Log("PC Face Mesh Visualizer Callback: " + topic);
parsePCFaceMesh = true;
color = new Color(0, 1, 0, alpha);
break;
case "/colorized_points_faced_4":
Debug.Log("PC Face Mesh Visualizer Callback: " + topic);
parsePCFaceMesh = true;
color = new Color(1, 1, 0, alpha);
break;
case "/colorized_points_faced_5":
Debug.Log("PC Face Mesh Visualizer Callback: " + topic);
parsePCFaceMesh = true;
color = new Color(1, 0, 0, alpha);
break;
default:
Debug.LogError("Topic not implemented: " + topic);
parsePCFaceMesh = true;
break;
}
if (parsePCFaceMesh)
{
PCFaceMsg meshMsg = new PCFaceMsg(raw_msg);
// Obtain visualizer for this topic and update mesh & color
PCFaceVisualizer visualizer = pcFaceVisualizers[topic];
visualizer.SetMesh(meshMsg);
visualizer.SetColor(color);
}
return(result);
}
