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;
}
}