public void RestartVisionListener()
{
if (vl.Listening)
{
vl.Stop();
}
VisionStatus.status = StatusButton.Status.Neutral;
vl = SetupVL();
}
void Start()
{
Debug.Log("Starting up! :D");
VisionPortField.text = VisionPort.ToString();
FootstepPortField.text = FootstepPort.ToString();
PosePortField.text = PosePort.ToString();
if (InitComms())
{
vl = SetupVL();
pl = SetupPL();
fl = SetupFL();
}
}
private LolaComms.VisionListener SetupVL()
{
Debug.Log("Setting up VisionListener...");
LolaComms.VisionListener vl = new LolaComms.VisionListener(VisionPort);
vl.onError += (errstr) =>
{
Debug.Log(errstr);
};
vl.onConnect += (host) =>
{
Debug.Log("Connected to: " + host);
};
vl.onDisconnect += (host) =>
{
Debug.Log("Disconnected from: " + host);
};
vl.onObstacleMessage += (msg) =>
{
Debug.Log("New obstacle message: " + msg.ToString());
switch (msg.action)
{
case (uint)LolaComms.Common.MsgId.SET_SSV:
{
if (msg.type == LolaComms.ObstacleType.Sphere)
{
if (obstacle_map.ContainsKey(msg.IdString()))
{
Debug.LogWarning("Received SET_SSV command for existing objec: " + msg.IdString());
Destroy(obstacle_map[msg.IdString()]);
obstacle_map.Remove(msg.IdString());
}
obstacle_map.Add(msg.IdString(), CreateSphere(msg.radius, new Vector3(msg.coeffs[0], msg.coeffs[1], msg.coeffs[2]), transform));
}
else if (msg.type == LolaComms.ObstacleType.Capsule)
{
if (obstacle_map.ContainsKey(msg.IdString()))
{
Debug.LogWarning("Received SET_SSV command for existing objec: " + msg.IdString());
Destroy(obstacle_map[msg.IdString()]);
obstacle_map.Remove(msg.IdString());
}
obstacle_map.Add(msg.IdString(),
CreateCapsule(msg.radius,
new Vector3(msg.coeffs[0], msg.coeffs[1], msg.coeffs[2]),
new Vector3(msg.coeffs[3], msg.coeffs[4], msg.coeffs[5]),
transform)
);
}
break;
}
case (uint)LolaComms.Common.MsgId.MODIFY_SSV:
{
if (obstacle_map.ContainsKey(msg.IdString()))
{
// in case we get a sphere replacing a capsule or vice versa, just replace what we had
Destroy(obstacle_map[msg.IdString()]);
if (msg.type == LolaComms.ObstacleType.Sphere)
{
obstacle_map[msg.IdString()] = CreateSphere(msg.radius, new Vector3(msg.coeffs[0], msg.coeffs[1], msg.coeffs[2]), transform);
}
else if (msg.type == LolaComms.ObstacleType.Capsule)
{
obstacle_map[msg.IdString()] = CreateCapsule(msg.radius,
new Vector3(msg.coeffs[0], msg.coeffs[1], msg.coeffs[2]),
new Vector3(msg.coeffs[3], msg.coeffs[4], msg.coeffs[5]),
transform);
}
}
else
{
Debug.LogWarning("Got MODIFY_SSV msg for non-existent object: " + msg.IdString());
if (msg.type == LolaComms.ObstacleType.Sphere)
{
obstacle_map.Add(msg.IdString(), CreateSphere(msg.radius, new Vector3(msg.coeffs[0], msg.coeffs[1], msg.coeffs[2]), transform));
}
else if (msg.type == LolaComms.ObstacleType.Capsule)
{
obstacle_map.Add(msg.IdString(),
CreateCapsule(msg.radius,
new Vector3(msg.coeffs[0], msg.coeffs[1], msg.coeffs[2]),
new Vector3(msg.coeffs[3], msg.coeffs[4], msg.coeffs[5]),
transform)
);
}
}
break;
}
case (uint)LolaComms.Common.MsgId.REMOVE_SSV_ONLY_PART:
{
if (obstacle_map.ContainsKey(msg.IdString()))
{
Destroy(obstacle_map[msg.IdString()]);
obstacle_map.Remove(msg.IdString());
}
else
{
Debug.LogWarning("Got REMOVE_SSV_ONLY_PART msg for non-existent object: " + msg.model_id);
}
break;
}
case (uint)LolaComms.Common.MsgId.REMOVE_SSV_WHOLE_SEGMENT:
{
List <string> to_remove = new List <string>();
foreach (var obstacle in obstacle_map) /// TODO: Evaluate whether we should use a different structure to optimize this case
{
if (obstacle.Key.Split('|')[0] == msg.model_id.ToString())
{
Destroy(obstacle.Value);
to_remove.Add(obstacle.Key);
}
}
if (to_remove.Count == 0)
{
Debug.LogWarning("Got REMOVE_SSV_WHOLE_SEGMENT msg for non-existent object: " + msg.model_id);
}
else
{
foreach (var item in to_remove)
{
obstacle_map.Remove(item);
}
}
break;
}
default:
Debug.LogWarning("Got unexpected SSV action: " + msg.action);
break;
}
};
vl.onSurfaceMessage += (msg) =>
{
Debug.Log("Got a new surface: " + msg.ToString());
switch (msg.action)
{
case (uint)LolaComms.Common.MsgId.SET_SURFACE: // add new surface
{
GameObject new_surface;
if (!surface_map.ContainsKey(msg.id)) // only create new object if this surface is really new
{
new_surface = Instantiate(Plane, this.transform);
}
else // if we have this surface already, replace the existing mesh data
{
new_surface = surface_map[msg.id];
}
var mesh = new_surface.GetComponent <MeshFilter>();
mesh.mesh = new Mesh();
// set vertices
var verts = new Vector3[msg.vertices.Length / 3];
for (int i = 0; i < verts.Length; i++)
{
verts[i] = new Vector3(msg.vertices[i * 3], msg.vertices[i * 3 + 1], msg.vertices[i * 3 + 2]);
}
mesh.mesh.vertices = verts;
// make triangle fan from vertices
var tris = new int[18]; // 6 triangles w/ 3 verts each
tris[0] = 0;
tris[1] = 1;
tris[2] = 2;
tris[3] = 0;
tris[4] = 2;
tris[5] = 3;
tris[6] = 0;
tris[7] = 3;
tris[8] = 4;
tris[9] = 0;
tris[10] = 4;
tris[11] = 5;
tris[12] = 0;
tris[13] = 5;
tris[14] = 6;
tris[15] = 0;
tris[16] = 6;
tris[17] = 7;
mesh.mesh.triangles = tris;
// set normals
var normals = new Vector3[verts.Length];
for (int i = 0; i < normals.Length; i++)
{
normals[i] = new Vector3(msg.normal[0], msg.normal[1], msg.normal[2]);
}
mesh.mesh.normals = normals;
if (surface_map.ContainsKey(msg.id)) // don't add surface if it already exists
{
Debug.LogWarning("Received SET_SURFACE msg for already existing surface ID: " + msg.id);
}
else
{
surface_map.Add(msg.id, new_surface);
}
mesh.mesh.RecalculateBounds();
break;
}
case (uint)LolaComms.Common.MsgId.MODIFY_SURFACE:
{
if (!surface_map.ContainsKey(msg.id))
{
Debug.LogError("Received MODIFY_SURFACE msg for non-existent surface ID: " + msg.id);
break;
}
var surface = surface_map[msg.id];
var mesh = surface.GetComponent <MeshFilter>();
mesh.mesh = new Mesh();
// set vertices
var verts = new Vector3[msg.vertices.Length / 3];
for (int i = 0; i < verts.Length; i++)
{
verts[i] = new Vector3(msg.vertices[i * 3], msg.vertices[i * 3 + 1], msg.vertices[i * 3 + 2]);
}
mesh.mesh.vertices = verts;
// make triangle fan from vertices
var tris = new int[18]; // 6 triangles w/ 3 verts each
tris[0] = 0;
tris[1] = 1;
tris[2] = 2;
tris[3] = 0;
tris[4] = 2;
tris[5] = 3;
tris[6] = 0;
tris[7] = 3;
tris[8] = 4;
tris[9] = 0;
tris[10] = 4;
tris[11] = 5;
tris[12] = 0;
tris[13] = 5;
tris[14] = 6;
tris[15] = 0;
tris[16] = 6;
tris[17] = 7;
mesh.mesh.triangles = tris;
// set normals
var normals = new Vector3[verts.Length];
for (int i = 0; i < normals.Length; i++)
{
normals[i] = new Vector3(msg.normal[0], msg.normal[1], msg.normal[2]);
}
mesh.mesh.normals = normals;
mesh.mesh.RecalculateBounds();
break;
}
case (uint)LolaComms.Common.MsgId.REMOVE_SURFACE:
{
if (surface_map.ContainsKey(msg.id))
{
Destroy(surface_map[msg.id]);
surface_map.Remove(msg.id);
}
else
{
Debug.LogWarning("Got REMOVE_SURFACE msg for non-existent surface: " + msg.id);
}
break;
}
case (uint)LolaComms.Common.MsgId.RESET_SURFACEMAP: // clear ALL surfaces
{
foreach (var surface in surface_map)
{
Destroy(surface.Value);
}
surface_map.Clear();
break;
}
default:
Debug.LogError("Got unknown surface action: " + msg.action);
break;
}
};
vl.Listen();
Debug.Log("VisionListener is listening: " + vl.Listening);
return(vl);
}
