public RobotData(int _robotID, string _robotName, EndeffectorData _endEffector)
{
this.robotID = _robotID;
this.robotName = _robotName;
this.endEffector = _endEffector;
this.robotPlane = new double[] { 0, 0, 0, 1, 0, 0, 0 };
}
// //public double[] TCPplane;
// //public int markerId;
public RobotData()
{
this.robotID = 0;
this.robotName = "Robot";
this.endEffector = null;
this.robotPlane = new double[] { 0, 0, 0, 1, 0, 0, 0 };
}
/// <summary>
/// This is the method that actually does the work.
/// </summary>
/// <param name="DA">The DA object is used to retrieve from inputs and store in outputs.</param>
protected override void SolveInstance(IGH_DataAccess DA)
{
// Get inputs.
string inputRobotName = "";
Mesh inputEndeffectorMesh = new Mesh();
int inputRobotID = 0;
Plane inputPlane = this.defaultPlane;
if (!DA.GetData(0, ref inputRobotName))
{
RobotOptionList();
}
DA.GetData(1, ref inputEndeffectorMesh);
DA.GetData(2, ref inputRobotID);
DA.GetData(3, ref inputPlane);
//////////////////////////////////////////////////////
// Process data.
inputEndeffectorMesh.Weld(Math.PI);
EndeffectorData endEffector = new EndeffectorData(MeshUtilities.EncodeMesh(inputEndeffectorMesh));
RobotData robot = new RobotData(inputRobotID, inputRobotName, endEffector, EncodeUtilities.EncodePlane(inputPlane));
UniversalDebug("Robot Object Created.");
//////////////////////////////////////////////////////
// Output.
DA.SetData(0, robot);
#if DEBUG
DA.SetData(1, this.debugMessages[this.debugMessages.Count - 1]);
#endif
}
private GameObject CreateBot(GameObject cPlane, GameObject goPrefab, EndeffectorData endEffector, int robotID) // int port, double[] tcp
{
#if DEBUG
Debug.Log("Robot: Instantiating");
#endif
GameObject goHoloBot = Instantiate(goPrefab, ObjectManager.instance.cPlane.transform.position, ObjectManager.instance.cPlane.transform.rotation, ObjectManager.instance.cPlane.transform);
goHoloBot.GetComponentInChildren <RobotController>().robotID = robotID;
foreach (MeshFilter meshFilter in goHoloBot.GetComponentsInChildren <MeshFilter>())
{
if (meshFilter.gameObject.tag == this.tagTool)
{
#if DEBUG
Debug.Log("Robot: Found Tool");
#endif
CreateMesh(endEffector, meshFilter);
}
}
return(goHoloBot);
}
// Decode Received Data.
public void ProcessRobot(List <RobotData> receivedRobots)
{
#if DEBUG
Debug.Log("Robot: got robots: " + receivedRobots.Count);
#endif
// Check for C-plane
if (!ObjectManager.instance.CheckCPlane())
{
return;
}
// Loop through all received robots.
for (int i = 0; i < receivedRobots.Count; i++)
{
int robotID = receivedRobots[i].robotID;
string bot = receivedRobots[i].robotName;
double[] basePlane = receivedRobots[i].robotPlane;
EndeffectorData endEffector = receivedRobots[i].endEffector;
#if DEBUG
Debug.Log("Robot: " + bot);
#endif
bool flagFound = false;
foreach (UnityRobot unityRobot in this.robotDictionary)
{
if (bot == unityRobot.name)
{
ProcessHoloBot(unityRobot.tag, unityRobot.goExample, basePlane, endEffector, robotID);
flagFound = true;
break;
}
}
if (!flagFound)
{
#if DEBUG
Debug.Log("Robot: robot not recognized");
#endif
}
}
}
private void CreateMesh(EndeffectorData endEffector, MeshFilter tool) //, double[] tcp
{
List <Vector3> vertices = new List <Vector3>();
List <int> triangles = new List <int>();
if (endEffector.vertices != null)
{
#if DEBUG
Debug.Log("Robot: Adding Verticies . . . !");
#endif
for (int j = 0; j < endEffector.vertices.Count; j++)
{
vertices.Add(new Vector3(endEffector.vertices[j][0],
endEffector.vertices[j][1],
endEffector.vertices[j][2]));
}
}
if (endEffector.faces != null)
{
#if DEBUG
Debug.Log("Robot: Adding Faces . . . !");
#endif
for (int j = 0; j < endEffector.faces.Count; j++)
{
triangles.Add(endEffector.faces[j][1]);
triangles.Add(endEffector.faces[j][2]);
triangles.Add(endEffector.faces[j][3]);
if ((endEffector.faces[j][0] == 1))
{
triangles.Add(endEffector.faces[j][1]);
triangles.Add(endEffector.faces[j][3]);
triangles.Add(endEffector.faces[j][4]);
}
}
}
tool.mesh = MeshUtilities.DecodeMesh(vertices, triangles);
tool.transform.Rotate(0, 180, -90);
}
private void ProcessHoloBot(string tag, GameObject goPrefab, double[] basePlane, EndeffectorData endEffector, int robotID)
{
GameObject goHoloBot;
//GameObject goHoloBot = GameObject.FindGameObjectWithTag(tag);
// If HoloBot not found - add it.
//if (goHoloBot == null) {
// #if DEBUG
// Debug.Log("Robot: robot doesn't exist. Creating.");
// #endif
// goHoloBot = CreateBot(ObjectManager.instance.cPlane, goPrefab, endEffector, robotID);
//}
if (!this.robotsInstantiated.ContainsKey(robotID))
{
goHoloBot = CreateBot(ObjectManager.instance.cPlane, goPrefab, endEffector, robotID);
this.robotsInstantiated.Add(robotID, goHoloBot);
}
else
{
goHoloBot = this.robotsInstantiated[robotID];
if (goHoloBot.tag != tag)
{
DestroyImmediate(goHoloBot);
goHoloBot = CreateBot(ObjectManager.instance.cPlane, goPrefab, endEffector, robotID);
this.robotsInstantiated[robotID] = goHoloBot;
}
}
// Update HoloBot transform.
goHoloBot.transform.SetPositionAndRotation(ObjectManager.instance.cPlane.transform.position + new Vector3((float)basePlane[0],
(float)basePlane[1],
(float)basePlane[2]),
ObjectManager.instance.cPlane.transform.rotation * new Quaternion(-(float)basePlane[5],
(float)basePlane[6],
(float)basePlane[4],
(float)basePlane[3]));
}
public RobotData(int _robotID, string _robotName, EndeffectorData _endEffector, double[] _robotPlane) : this(_robotID, _robotName, _endEffector)
{
this.robotPlane = _robotPlane;
}
