private void executeAddServo(object o)
{
PololuMaestroServo servo = new PololuMaestroServo();
servo.setOwner(this);
ServoList.Add(servo);
}
////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////
///
/// PARSING METHODS
///
////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////
#region CALIB_PARSING
private void ParseAllCalibrationData()
{
//////////////////////////////////////////////////////////////////////////////////////////////////
// Build All Hololens Sensors (VLC and PV)\
//////////////////////////////////////////////////////////////////////////////////////////////////
{
TextAsset jText = Resources.Load(HololensSensorCalibFile) as TextAsset;
JObject sensorCalibJObj = JObject.Parse(jText.text);
foreach (var name in m_sensorNames)
{
var sensor = ExtractSensorInformation(sensorCalibJObj, name);
HololensSensorList.Add(name, sensor);
}
}
// Visualize structure;
m_sensorStructure = new GameObject();
m_sensorStructure.name = "CameraSensorStructure";
m_sensorStructure.transform.parent = m_AARCameraProjectorRig.transform;
m_sensorStructure.transform.localPosition = Vector3.zero;
foreach (var sensor in HololensSensorList)
{
GameObject obj = (SensorModel == null) ?
GameObject.CreatePrimitive(PrimitiveType.Sphere) :
GameObject.Instantiate(SensorModel);
obj.name = sensor.Key.ToString();
obj.transform.parent = m_sensorStructure.transform;
obj.transform.localPosition = Vector3.zero;
obj.transform.localPosition = sensor.Value.GetPositionHololensFrameOfReference();
obj.transform.rotation = sensor.Value.GetRotationHololensFrameOfReference();
sensor.Value.AttachGameObject(obj);
}
//////////////////////////////////////////////////////////////////////////////////////////////////
// Build servo and projector
//////////////////////////////////////////////////////////////////////////////////////////////////
// TODO: Attach the servo and projector scripts directly to these game objects
// TODO: Create projector script that can take in the json format
{
TextAsset jText = Resources.Load(ServoProjectorCalibFile) as TextAsset;
JObject servoProCalib = JObject.Parse(jText.text);
// Servo
var servos = servoProCalib["Servos"];
foreach (var servoName in ServoNames)
{
var servo = ExtractServoInformation(servos, servoName);
ServoList.Add(servoName, servo);
}
// Projector
var projectors = servoProCalib["ProjectorViews"];
foreach (var projectorName in ProjectorNames)
{
var projector = ExtractProjectorInformation(projectors, projectorName);
ProjectorList.Add(projectorName, projector);
}
}
// Create structure on hololens
m_servoProjectorStructure = new GameObject();
m_servoProjectorStructure.name = "ServoProjectorStructure";
m_servoProjectorStructure.transform.parent = m_AARCameraProjectorRig.transform;
m_servoProjectorStructure.transform.localPosition = Vector3.zero;
// Servos (ordering matters!)
AARServo lastAddedServo = null;
foreach (var servoName in ServoNames)
{
AARServo servo = ServoList[servoName];
// Servo Object
GameObject servoObj = new GameObject();
servoObj.name = servo.GetName();
servoObj.transform.parent = m_servoProjectorStructure.transform;
servoObj.transform.localPosition = Vector3.zero;
// Set positio and rotation based on calibration
var pvCamera = HololensSensorList[
Util.SensorTypeToString(HololensSensorType.AAR_SENSOR_PVCAMERA)
];
Matrix4x4 coordinateChange = Matrix4x4.Rotate(Quaternion.Euler(0, 0, 180));
Vector4 p = new Vector4(0, 0, 0, 1);
Matrix4x4 servoToHololens =
coordinateChange *
pvCamera.GetLocalToHololensFrameOfReferenceMatrix() *
servo.GetLocalToPVCameraMatrix() *
coordinateChange;
servoObj.transform.localPosition = servoToHololens * p;
servoObj.transform.localRotation = servoToHololens.rotation;
// Check if there is a previous servo for linking
if (lastAddedServo != null)
{
GameObject previousServo = lastAddedServo.GetRotationGameObject();
servoObj.transform.parent = previousServo.transform;
}
servo.AttachGameObject(servoObj);
lastAddedServo = servo;
}
// Projectors
foreach (var projector in ProjectorList)
{
GameObject projectoObj = new GameObject();
projectoObj.name = projector.Key.ToString();
projectoObj.transform.parent = m_servoProjectorStructure.transform;
projectoObj.transform.localPosition = Vector3.zero;
// Set positon and rotation based on calibration
var pvCamera = HololensSensorList[
Util.SensorTypeToString(HololensSensorType.AAR_SENSOR_PVCAMERA)
];
Vector4 p = new Vector4(0, 0, 0, 1);
Matrix4x4 coordinateChange = Matrix4x4.Rotate(Quaternion.Euler(0, 0, 180));
Matrix4x4 servoToHololens =
coordinateChange *
pvCamera.GetLocalToHololensFrameOfReferenceMatrix() *
projector.Value.GetLocalToPVCameraMatrix(); // * coordinateChange;
// Set local positions
projectoObj.transform.localPosition = servoToHololens * p;
projectoObj.transform.localRotation = servoToHololens.rotation;
// Attach to last seen servo
if (lastAddedServo != null)
{
projectoObj.transform.parent = lastAddedServo.GetRotationGameObject().transform;
}
projector.Value.AttachGameObject(projectoObj);
// Model
GameObject model = (ProjectorModel == null) ?
GameObject.CreatePrimitive(PrimitiveType.Sphere) :
GameObject.Instantiate(ProjectorModel);
projector.Value.SetModel(model);
}
}