public override string ToYAMLString()
{
string array = "[";
for (int i = 0; i < _covariance.Length; i++)
{
array = array + _covariance[i].ToString("G", System.Globalization.CultureInfo.InvariantCulture);
if (i < _covariance.Length - 1)
{
array += ",";
}
}
array += "]";
return("{\"pose\" : " + _pose.ToYAMLString() + ", \"covariance\" : " + array + "}");
}
public override string ToYAMLString()
{
string array = "[";
for (int i = 0; i < _covariance.Length; i++)
{
array = array + _covariance[i].ToString();
if (_covariance.Length - i <= 1)
{
array += ",";
}
}
array += "]";
return("{\"pose\" : " + _pose.ToYAMLString() + ", \"covariance\" : " + array + "}");
}
public override string ToYAMLString()
{
string flagsString = "[";
for (int i = 0; i < _flags.Count; i++)
{
flagsString = flagsString + _flags[i].ToYAMLString();
if (_flags.Count - i > 1)
{
flagsString += ",";
}
}
flagsString += "]";
return("{\"object_id\":\"" + _object_id + "\"" +
", \"object_type\":\"" + _object_type + "\"" +
", \"pose\":" + _pose.ToYAMLString() +
", \"flags\":" + flagsString +
", \"on_table\":" + _on_table.ToString().ToLower() + "}");
}
public override string ToYAMLString()
{
string array = "[";
for (int i = 0; i < _body_part_3d.Length; i++)
{
array = array + _body_part_3d[i].ToYAMLString();
if (_body_part_3d.Length - i <= 1)
{
array += ",";
}
}
array += "]";
return("{\"name\" : " + _name
+ ", \"u\" : " + _u.ToString("F", System.Globalization.CultureInfo.InvariantCulture)
+ ", \"v\" : " + _v.ToString("F", System.Globalization.CultureInfo.InvariantCulture)
+ ", \"w\" : " + _v.ToString("F", System.Globalization.CultureInfo.InvariantCulture)
+ ", \"h\" : " + _v.ToString("F", System.Globalization.CultureInfo.InvariantCulture)
+ ", \"pose_3D\" : " + _pose_3D.ToYAMLString()
+ ", \"certainty\" : " + _certainty.ToString("F", System.Globalization.CultureInfo.InvariantCulture)
+ ", \"body_part_3d\" : " + array
+ "}");
}
public override string ToYAMLString()
{
return("{\"id\" : \"" + _id + "\", \"idRoom\" : \"" + _idRoom + "\", \"type\" : \"" + _type + "\", \"score\" : " + _score.ToString("N", System.Globalization.CultureInfo.InvariantCulture) + ", \"pointCloud\" : " + _pointCloud.ToYAMLString() + ", \"pose\" : " + _pose.ToYAMLString() + ", \"scale\" : " + _scale.ToYAMLString() + "}");
}
public override string ToYAMLString()
{
return("{\"header\" : " + _header.ToYAMLString() + ", \"pose\" : " + _pose.ToYAMLString() + "}");
}
public override string ToYAMLString()
{
return("{\"name\" : \"" + _name + "\""
+ ", \"interaction_pose\" : " + _interaction_pose.ToYAMLString()
+ "}");
}
public override string ToYAMLString()
{
return("{\"map_load_time\" : " + _map_load_time.ToYAMLString() + ", \"resolution\" : " + _resolution.ToString("F", System.Globalization.CultureInfo.InvariantCulture) + ", \"width\" : " + _width + ", \"height\" : " + _height + ", \"origin\" : " + _origin.ToYAMLString() + "}");
}
// Update is called once per frame
void Update()
{
if (SystemStarter.Instance.calibrated)
{
if (interfaceStateMsg != null)
{
//pick from polygon editing
if (interfaceStateMsg.GetSystemState() == InterfaceStateMsg.SystemState.STATE_LEARNING && programItemMsg.GetIType() == "PlaceToPose" &&
interfaceStateMsg.GetEditEnabled() == true)
{
//check that object type is set
if (programMsg == null && !serviceCalled)
{
ROSCommunicationManager.Instance.ros.CallService("/art/db/program/get", "{\"id\": " + interfaceStateMsg.GetProgramID() + "}");
serviceCalled = true;
}
else if (programMsg != null && !objTypeReferenceSet)
{
ProgramBlockMsg programBlockMsg = programMsg.GetBlockByID(interfaceStateMsg.GetBlockID());
ProgramItemMsg refItem = programBlockMsg.GetProgramItemByID(programItemMsg.GetRefID()[0]);
if (refItem.GetObject().Count == 0 && !sayUnknownObjectType)
{
//TextToSpeechManager.Instance.Speak(Texts.PlaceToPoseIE_PickIsNotProgrammed);
sayUnknownObjectType = true;
}
else if (refItem.GetObject().Count > 0)
{
objTypeReferenceSet = true;
}
}
//if object type is set
if (!sayAdjustArea && objTypeReferenceSet)
{
//TextToSpeechManager.Instance.Speak(Texts.PlaceToPoseIE_DragOBjectOutline);
sayAdjustArea = true;
}
//show hand and play it's animation
if (!pointingHand.activeSelf && !animationShowed)
{
//pointingHand.SetActive(true);
originalPoseMsg = programItemMsg.GetPose()[0].GetPose();
//get middle point of bottom line
spawnPoint = new Vector3(programItemMsg.GetPose()[0].GetPose().GetPosition().GetX(),
-programItemMsg.GetPose()[0].GetPose().GetPosition().GetY(),
programItemMsg.GetPose()[0].GetPose().GetPosition().GetZ());
//ARTABLE BUG - place pose not actualizing interface state.. initially set to 0.. if so, set spawn point on the middle of the table (where it appears)
if (spawnPoint.Equals(new Vector3(0f, 0f, 0f)))
{
spawnPoint = spawnPointOnTable;
}
movePoint = spawnPoint + new Vector3(0f, 0.15f, 0f);
//pointingHand.transform.localPosition = spawnPoint;
pointingHand.GetComponent <PointingHandMover>().Run(spawnPoint, movePoint);
}
//if pose points are same, then user didn't moved with it.. so play hand animation
if (originalPoseMsg.ToYAMLString().Equals(programItemMsg.GetPose()[0].GetPose().ToYAMLString()))
{
//pointingHand.transform.localPosition = Vector3.Lerp(pointingHand.transform.localPosition, movePoint, Time.deltaTime * 1.5f);
//if (ObjectInPosition(pointingHand, movePoint, 0.0005f)) {
// pointingHand.transform.localPosition = spawnPoint;
//}
}
else
{
//pointingHand.SetActive(false);
pointingHand.GetComponent <PointingHandMover>().Stop();
animationShowed = true;
}
//check if everything is set for this instruction
if (objTypeReferenceSet && programItemMsg.GetPose()[0].GetPose()._position.GetX() != 0.0f)
{
instructionProgrammed = true;
}
}
//reset all variables
else
{
if (instructionProgrammed)
{
//TextToSpeechManager.Instance.Speak(Texts.PlaceToPoseIE_GoodJob);
instructionProgrammed = false;
pointingHand.GetComponent <PointingHandMover>().Stop();
}
//if just object type reference is set but not place pose
else if (objTypeReferenceSet)
{
//TextToSpeechManager.Instance.Speak(Texts.PlaceToPoseIE_ForgotPlacePose);
pointingHand.GetComponent <PointingHandMover>().Stop();
}
sayAdjustArea = false;
sayUnknownObjectType = false;
objTypeReferenceSet = false;
serviceCalled = false;
animationShowed = false;
programMsg = null;
}
}
}
}
