public static void GUI(this PolygonMsg message)
{
GUILayout.Label($"({message.points.Length} points):");
foreach (Point32Msg p in message.points)
{
GUI(p);
}
}
private static bool PolygonSet(PolygonMsg polygon)
{
if (polygon.GetPoints().Length == 0)
{
return(false);
}
return(true);
}
private void InitPlacePose()
{
//placePose = programItem.GetPose()[0].GetPose();
//placePosition = new Vector3(placePose.GetPosition().GetX(), -placePose.GetPosition().GetY(), placePose.GetPosition().GetZ() - 0.023f);
//placePosition = new Vector3(placePose.GetPosition().GetX(), -placePose.GetPosition().GetY(), 0f);
//placeQuaternion = new Quaternion(-placePose.GetOrientation().GetX(), placePose.GetOrientation().GetY(), -placePose.GetOrientation().GetZ(), placePose.GetOrientation().GetW());
//upPosition = objectToPlace.transform.localPosition + new Vector3(0,0,0.1f);
PolygonMsg polygonMsg = programItem.GetPolygon()[0].GetPolygon();
//compute centroid of polygon from which to pick
PointMsg[] _points = polygonMsg.GetPoints();
List <Vector2> points = new List <Vector2>();
for (int i = 0; i < _points.Length; i++)
{
points.Add(new Vector2(_points[i].GetX(), _points[i].GetY()));
}
centroid = GetCentroid(points);
placePosition = new Vector3(centroid.x, -centroid.y, objectToPlace.transform.GetChild(0).transform.localScale.x / 2f);
//rotate object to lay down on the table and to be parallel with world_anchor
if (objectToPlace.name.Equals("Stretcher") || objectToPlace.name.Equals("LongLeg") || objectToPlace.name.Equals("ShortLeg") ||
objectToPlace.name.Equals("Spojka") || objectToPlace.name.Equals("Dlouha_noha") || objectToPlace.name.Equals("Kratka_noha"))
{
placeQuaternion = Quaternion.Euler(new Vector3(-90f, 0f, 0f));
}
else
{
placeQuaternion = Quaternion.Euler(new Vector3(0f, 0f, 0f));
}
//get list of objects in place polygon in order to turn collisions off with the virtual rigidbody object
objectsInPolygon = ObjectsManager.Instance.GetObjectsFromPolygon(points);
//place it directly to container
foreach (var obj in objectsInPolygon)
{
if (obj.name.Contains("kontejner") || obj.name.Contains("container"))
{
placePosition = obj.transform.localPosition;
break;
}
}
upPosition = placePosition + new Vector3(0, 0, 0.3f);
}
public PolygonStampedMsg(HeaderMsg header, PolygonMsg polygon)
{
_header = header;
_polygon = polygon;
}
public PolygonStampedMsg(JSONNode msg)
{
_header = new HeaderMsg(msg["header"]);
_polygon = new PolygonMsg(msg["polygon"]);
}
private void InitObjectToPick()
{
List <GameObject> objectsInPolygon = new List <GameObject>();
//TODO: overit, ze v polygonu neni virtualni objekt .. pokud je, tak beru ho
PolygonMsg polygonMsg = programItem.GetPolygon()[0].GetPolygon();
//compute centroid of polygon from which to pick
PointMsg[] _points = polygonMsg.GetPoints();
List <Vector2> points = new List <Vector2>();
for (int i = 0; i < _points.Length; i++)
{
points.Add(new Vector2(_points[i].GetX(), _points[i].GetY()));
}
centroid = GetCentroid(points);
//find all virtual objects that might be in given polygon
objectsInPolygon = ObjectsManager.Instance.GetVirtualObjectsFromPolygon(points, programItem.GetObject()[0]);
//HIGHEST PRIORITY - we have some virtual objects placed in polygon
if (objectsInPolygon.Count > 0)
{
foreach (GameObject obj in objectsInPolygon)
{
if (obj.name.Equals(programItem.GetObject()[0]))
{
objectToPick = obj;
break;
}
}
}
//If no object created yet, then create new one and set everything needed for the freshly created object
else if (objectToPick == null)
{
//create empty parent object that has scale 1,1,1 .. when arm attaches it, it won't deform (cube for picking has different scale and it caused mess when arm attached it)
objectToPick = new GameObject();
//objectToPick is created at [0,0,0] world coords
//making him child of world_anchor leaves him still at [0,0,0] world coords.. but shifts it's localPosition to be in respect to world_anchor
//setting localPosition to [0,0,0] would make it appear on the same position as world_anchor
objectToPick.transform.parent = world_anchor.transform;
//instantiate actual object to pick and attach it to empty parent
GameObject childObjectToPick = Instantiate(visualizeObjectPrefab, objectToPick.transform);
childObjectToPick.transform.localScale = ObjectsManager.Instance.getObjectDimensions(programItem.GetObject()[0]);
childObjectToPick.transform.parent = objectToPick.transform;
objectToPick.transform.localPosition = new Vector3(0, 0, childObjectToPick.transform.localScale.x / 2f);
objectToPick.name = programItem.GetObject()[0];
//add reference on created object into ObjectsManagers list
ObjectsManager.Instance.virtualObjectList.Add(objectToPick);
//apply this centroid to init of object to pick
objectToPick.transform.localPosition = new Vector3(centroid.x, -centroid.y, objectToPick.transform.GetChild(0).transform.localScale.x / 2f);
//rotate object to lay down on the table and to be parallel with world_anchor
if (objectToPick.name.Equals("Stretcher") || objectToPick.name.Equals("LongLeg") || objectToPick.name.Equals("ShortLeg") ||
objectToPick.name.Equals("Spojka") || objectToPick.name.Equals("Dlouha_noha") || objectToPick.name.Equals("Kratka_noha"))
{
objectToPick.transform.localEulerAngles = new Vector3(-90f, 0f, 0f);
}
else
{
objectToPick.transform.localEulerAngles = new Vector3(0f, 0f, 0f);
}
}
}
// Update is called once per frame
void Update()
{
if (SystemStarter.Instance.calibrated)
{
if (interfaceStateMsg != null)
{
//pick from polygon editing
if (interfaceStateMsg.GetSystemState() == InterfaceStateMsg.SystemState.STATE_LEARNING && programItemMsg.GetIType() == "PickFromPolygon" &&
interfaceStateMsg.GetEditEnabled() == true)
{
//if no object is selected
if (programItemMsg.GetObject().Count == 0)
{
//if no objects are placed on the table
//TODO: pokud je objekt detekovan v podavaci.. pocita se.. nemel by.. zkusit kontrolovat aj z souradnici
if (!ObjectsManager.Instance.AnyObjectIsOnTable() && !sayPlaceObject)
{
//TextToSpeechManager.Instance.Speak(Texts.PickFromPolygonIE_NoObjectsOnTable);
sayPlaceObject = true;
//in case that user removes, places and removes objects from the table
saySelectObject = false;
}
//some object is placed on the table
if (ObjectsManager.Instance.AnyObjectIsOnTable())
{
if (!saySelectObject)
{
//TextToSpeechManager.Instance.Speak(Texts.PickFromPolygonIE_SelectObjectType);
saySelectObject = true;
//in case that user removes objects from the table
sayPlaceObject = false;
//init pointing hands
objectsOnTable = ObjectsManager.Instance.GetObjectsFromTable();
SpawnAndRunPointingHands();
}
}
sayReward = true;
}
//if some object is selected
else if (programItemMsg.GetObject().Count > 0)
{
StopAndDestroyPointingHands();
if (sayReward)
{
//TextToSpeechManager.Instance.Speak(Texts.Perfect);
sayReward = false;
}
if (!sayAdjustArea)
{
//TextToSpeechManager.Instance.Speak(Texts.PickFromPolygonIE_AdjustPickArea);
sayAdjustArea = true;
}
//show hand and play it's animation
if (!pointingHand.activeSelf && !animationShowed)
{
//pointingHand.SetActive(true);
originalPolygonMsg = programItemMsg.GetPolygon()[0].GetPolygon();
// 4-------3
// | |
// | |
// 1-------2
//load first point of polygon
Vector3 firstPoint = new Vector3(programItemMsg.GetPolygon()[0].GetPolygon().GetPoints()[0].GetX(),
-programItemMsg.GetPolygon()[0].GetPolygon().GetPoints()[0].GetY(),
programItemMsg.GetPolygon()[0].GetPolygon().GetPoints()[0].GetZ());
Vector3 secondPoint = new Vector3(programItemMsg.GetPolygon()[0].GetPolygon().GetPoints()[1].GetX(),
-programItemMsg.GetPolygon()[0].GetPolygon().GetPoints()[1].GetY(),
programItemMsg.GetPolygon()[0].GetPolygon().GetPoints()[1].GetZ());
//get middle point of bottom line
spawnPoint = firstPoint + (secondPoint - firstPoint) / 2 + new Vector3(0f, -0.05f, 0f);
movePoint = spawnPoint + new Vector3(0f, 0.15f, 0f);
//pointingHand.transform.localPosition = spawnPoint;
pointingHand.GetComponent <PointingHandMover>().Run(spawnPoint, movePoint);
}
//if polygon points are same, then user didn't moved with it.. so play hand animation
if (originalPolygonMsg.ToYAMLString().Equals(programItemMsg.GetPolygon()[0].GetPolygon().ToYAMLString()))
{
//pointingHand.transform.localPosition = Vector3.Lerp(pointingHand.transform.localPosition, movePoint, Time.deltaTime * 1.5f);
//if (ObjectInPosition(pointingHand, movePoint, 0.0005f)) {
// pointingHand.transform.localPosition = spawnPoint;
//}
}
else
{
//pointingHand.SetActive(false);
pointingHand.GetComponent <PointingHandMover>().Stop();
animationShowed = true;
}
//check if everything is set for this instruction
instructionProgrammed = true;
}
}
//reset all variables
else
{
if (instructionProgrammed)
{
//TextToSpeechManager.Instance.Speak(Texts.PickFromPolygonIE_GoodJob);
instructionProgrammed = false;
pointingHand.GetComponent <PointingHandMover>().Stop();
}
//if user ended programming without doing anything
if (pointingHandsList.Count > 0)
{
StopAndDestroyPointingHands();
}
sayReward = false;
sayPlaceObject = false;
saySelectObject = false;
sayAdjustArea = false;
animationShowed = false;
}
}
}
}
//if there is no detected or virtual object in polygon, then it creates completely new one
private void InitObjectsToDrill()
{
objectsToDrill = new List <GameObject>();
objectsToDrillReferenceHolder = new List <GameObject>();
//get polygon points from current message
PolygonMsg polygonMsg = programItem.GetPolygon()[0].GetPolygon();
PointMsg[] _points = polygonMsg.GetPoints();
List <Vector2> points = new List <Vector2>();
for (int i = 0; i < _points.Length; i++)
{
points.Add(new Vector2(_points[i].GetX(), _points[i].GetY()));
}
//find all virtual objects that might be in given polygon
objectsInPolygon = ObjectsManager.Instance.GetVirtualObjectsFromPolygon(points, programItem.GetObject()[0]);
//HIGHEST PRIORITY - we have some virtual objects placed in polygon
if (objectsInPolygon.Count > 0)
{
objectsToDrill = new List <GameObject>(objectsInPolygon);
objectsToDrillReferenceHolder = new List <GameObject>(objectsInPolygon);
}
//MID PRIORITY - we try to find real objects that are in polygon
else
{
//find all physical objects that are in given polygon
objectsInPolygon = ObjectsManager.Instance.GetObjectsFromPolygon(points, programItem.GetObject()[0]);
//if in polygon are some detected objects .. instatiate on their poses virtual ones and drill all of them
if (objectsInPolygon.Count > 0)
{
Debug.Log("POLYGON NOT EMPTY!");
foreach (GameObject obj in objectsInPolygon)
{
GameObject objectToDrill = new GameObject();
objectToDrill.transform.parent = world_anchor.transform;
objectToDrill.transform.localPosition = obj.transform.localPosition;
objectToDrill.transform.localRotation = obj.transform.localRotation;
objectToDrill.name = programItem.GetObject()[0];
//instantiate actual object to drill and attach it to empty parent
GameObject childObjectToDrill = Instantiate(visualizeObjectPrefab, objectToDrill.transform);
childObjectToDrill.transform.localScale = ObjectsManager.Instance.getObjectDimensions(programItem.GetObject()[0]);
childObjectToDrill.transform.parent = objectToDrill.transform;
objectsToDrill.Add(objectToDrill);
objectsToDrillReferenceHolder.Add(objectToDrill);
//add reference on created object into ObjectsManagers list
ObjectsManager.Instance.virtualObjectList.Add(objectToDrill);
}
}
//LOWEST PRIORITY - if there are no objects in polygon.. then create virtual one
else
{
Debug.Log("POLYGON IS EMPTY!");
//create empty parent object that has scale 1,1,1 .. when arm attaches it, it won't deform (cube for picking has different scale and it caused mess when arm attached it)
GameObject objectToDrill = new GameObject();
//objectToDrill is created at [0,0,0] world coords
//making him child of world_anchor leaves him still at [0,0,0] world coords.. but shifts it's localPosition to be in respect to world_anchor
//setting localPosition to [0,0,0] would make it appear on the same position as world_anchor
objectToDrill.transform.parent = world_anchor.transform;
objectToDrill.transform.localPosition = new Vector3(0, 0, 0);
objectToDrill.name = programItem.GetObject()[0];
//compute centroid of polygon in which to drill
centroid = GetCentroid(points);
//apply this centroid to init of object to pick
objectToDrill.transform.localPosition = new Vector3(centroid.x, -centroid.y, 0);
//rotate object to lay down on the table and to be parallel with world_anchor
objectToDrill.transform.localEulerAngles = new Vector3(-90f, 0f, 0f);
//instantiate actual object to drill and attach it to empty parent
GameObject childObjectToDrill = Instantiate(visualizeObjectPrefab, objectToDrill.transform);
childObjectToDrill.transform.localScale = ObjectsManager.Instance.getObjectDimensions(programItem.GetObject()[0]);
childObjectToDrill.transform.parent = objectToDrill.transform;
objectsToDrill.Add(objectToDrill);
objectsToDrillReferenceHolder.Add(objectToDrill);
//add reference on created object into ObjectsManagers list
ObjectsManager.Instance.virtualObjectList.Add(objectToDrill);
}
}
}
