예제 #1
0
    public void Visualize()
    {
        if (ProgramHelper.ItemLearned(programItemMsg))
        {
            Debug.Log("Visualizing PICK_FROM_FEEDER");

            //if (objectToPickUnmanipulatable != null)
            //    Destroy(objectToPickUnmanipulatable);

            //convert ros coordinate system to unity coordinate system
            Vector3    robotArmPosition = ROSUnityCoordSystemTransformer.ConvertVector(programItemMsg.GetPose()[0].GetPose().GetPosition().GetPoint());
            Quaternion robotArmRotation = ROSUnityCoordSystemTransformer.ConvertQuaternion(programItemMsg.GetPose()[0].GetPose().GetOrientation().GetQuaternion());

            //show robot arm only if it's not grasping object
            if (!RobotHelper.HasArmGraspedObject(robotArmPosition.x > MainMenuManager.Instance.currentSetup.GetTableWidth() / 2f ?
                                                 RobotHelper.RobotArmType.LEFT_ARM : RobotHelper.RobotArmType.RIGHT_ARM))
            {
                robotArm.transform.localPosition = robotArmPosition;
                robotArm.transform.localRotation = robotArmRotation;
                robotArm.GetComponent <PR2GripperController>().SetArmActive(true);
                robotArm.GetComponent <PR2GripperController>().CloseGripperInstantly();
            }

            Vector3 objectPosition = FakeFeederObjectsPositions.GetObjectPositionInFeeder(programItemMsg.GetObject()[0],
                                                                                          (robotArmPosition.x > MainMenuManager.Instance.currentSetup.GetTableWidth() / 2f ? FakeFeederObjectsPositions.FeederType.right_feeder : FakeFeederObjectsPositions.FeederType.left_feeder));
            Quaternion objectOrientation = FakeFeederObjectsPositions.GetObjectOrientationInFeeder(programItemMsg.GetObject()[0],
                                                                                                   (robotArmPosition.x > MainMenuManager.Instance.currentSetup.GetTableWidth() / 2f ? FakeFeederObjectsPositions.FeederType.right_feeder : FakeFeederObjectsPositions.FeederType.left_feeder));
            Vector3 objectDims = ObjectsManager.Instance.GetObjectTypeDimensions(programItemMsg.GetObject()[0]);

            //objectToPickUnmanipulatable = Instantiate(BasicObjectUnmanipulatablePrefab, world_anchor.transform);
            //objectToPickUnmanipulatable.transform.localPosition = ROSUnityCoordSystemTransformer.ConvertVector(objectPosition);
            //objectToPickUnmanipulatable.transform.localRotation = ROSUnityCoordSystemTransformer.ConvertQuaternion(objectOrientation);
            //objectToPickUnmanipulatable.transform.GetChild(0).transform.localScale = objectDims;

            Debug.Log("BEFORE INSTANTIATING");
            if (objectToPick == null || objectToPick.Equals(null))
            {
                Debug.Log("INSTANTIATING");
                objectToPick = Instantiate(BasicObjectManipulatablePrefab, world_anchor.transform);
            }
            objectToPick.GetComponent <ObjectManipulationEnabler>().DisableManipulation();
            objectToPick.transform.localPosition = ROSUnityCoordSystemTransformer.ConvertVector(objectPosition);
            objectToPick.transform.localRotation = ROSUnityCoordSystemTransformer.ConvertQuaternion(objectOrientation);
            objectToPick.transform.GetChild(0).transform.localScale = objectDims;
        }
    }
예제 #2
0
    private GameObject InitObjectInFeeder(FakeFeederObjectsPositions.FeederType feederType, string objectType)
    {
        GameObject objectInFeeder = Instantiate(feederObjectPrefab, world_anchor.transform);

        objectInFeeder.transform.localPosition = ROSUnityCoordSystemTransformer.ConvertVector(FakeFeederObjectsPositions.GetObjectPositionInFeeder(objectType, feederType));
        objectInFeeder.transform.localRotation = ROSUnityCoordSystemTransformer.ConvertQuaternion(FakeFeederObjectsPositions.GetObjectOrientationInFeeder(objectType, feederType));
        objectInFeeder.GetComponent <DetectedObject>().SetObject(
            ROSUnityCoordSystemTransformer.ConvertVector(FakeFeederObjectsPositions.GetObjectPositionInFeeder(objectType, feederType)),
            ROSUnityCoordSystemTransformer.ConvertQuaternion(FakeFeederObjectsPositions.GetObjectOrientationInFeeder(objectType, feederType)),
            ObjectsManager.Instance.GetObjectTypeDimensions(objectType),
            objectType,
            0);
        objectInFeeder.name = feederType + "_" + objectType;
        objectInFeeder.tag  = "feeder_object";
        objectInFeeder.SetActive(false);

        return(objectInFeeder);
    }