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