示例#1
0
    private static bool PolygonSet(PolygonMsg polygon)
    {
        if (polygon.GetPoints().Length == 0)
        {
            return(false);
        }

        return(true);
    }
示例#2
0
    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);
    }
示例#3
0
    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);
            }
        }
    }
示例#4
0
    //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);
            }
        }
    }