public void SetProgramMsgFromROS(ProgramMsg msg)
{
//receive only if program is really visualizing (conflicts when in edit mode)
if (start_visualization)
{
programMsg = msg;
}
//Debug.Log(programMsg.ToYAMLString());
}
public void SetProgramMsgFromROS(ProgramMsg msg)
{
//set only if system is really in edit mode of this particullar instruction
if (interfaceStateMsg != null)
{
if (interfaceStateMsg.GetSystemState() == InterfaceStateMsg.SystemState.STATE_LEARNING && programItemMsg.GetIType() == "PlaceToPose" &&
interfaceStateMsg.GetEditEnabled() == true)
{
programMsg = msg;
}
}
}
public static void SetProgramMsgFromROS(ProgramMsg msg)
{
LoadingProgram = false;
//Debug.Log("PH new program loaded");
currentProgram = msg;
if (OnInterfaceStateChanged != null)
{
OnInterfaceStateChanged(interfaceStateMsg);
}
}
//TODO cannot jump by index, but through dictionary
//private int GetCorrectIndexOfPreviousInstruction(int i) {
// //1st decrement - replay current instruction
// //2nd decrement - replay previous instruction
// //3rd decrement - only if replayed instruction was place to pose
// //skip first instruction
// if (i > 0) {
// i--;
// //if instruction before should be PLACE_TO_POSE than go one more instruction backwards to have something to place (PickFromFeeder or PickFromPolygon)
// //it would be nonsense to replay place to pose when robot has nothing in gripper to place
// if (programToVisualize.ElementAt(i).Value.gameObject.tag.Equals("PLACE_TO_POSE")) {
// i--;
// }
// }
// //if it was first instruction - replay it
// //if it was any other instruction - replay previous instruction
// i--;
// return i;
//}
//clears all variables and destroyes program instructions.. should be called only when "Back to blocks" pressed
public void ClearProgram()
{
foreach (var block in program.Values)
{
foreach (var programItem in block.Values)
{
Destroy(programItem);
}
}
program.Clear();
programHelper.Clear();
programMsg = null;
programBlockMsg = null;
}
private void CreateFakeProgramMsg()
{
programMsg = new ProgramMsg(new ProgramHeaderMsg(13, "Training - polygon", "", false), new List <ProgramBlockMsg>()
{
new ProgramBlockMsg(1, "Zvedni ze stolu a poloz", new List <ProgramItemMsg> {
new ProgramItemMsg(1, 2, 0, "PickFromPolygon", "", new List <string>()
{
"ShortLeg"
}, new List <PoseStampedMsg>(),
new List <PolygonStampedMsg>()
{
new PolygonStampedMsg(new HeaderMsg(0, new TimeMsg(0, 0), "marker"), new PolygonMsg(new PointMsg[] {
new PointMsg(0.9060779f, 0.2258368f, 0f), new PointMsg(1.17369f, 0.0966467f, 0f), new PointMsg(1.308086f, 0.5375652f, 0f),
new PointMsg(1.020977f, 0.5331101f, 0f)
}))
},
new List <ushort>(), new List <KeyValueMsg>(), new List <string>(), new List <SceneLabelMsg>()),
new ProgramItemMsg(2, 3, 0, "PlaceToPose", "", new List <string>(), new List <PoseStampedMsg>()
{
new PoseStampedMsg(new HeaderMsg(0, new TimeMsg(0, 0), "marker"),
new ROSBridgeLib.geometry_msgs.PoseMsg(new PointMsg(0.4221045f, 0.378173f, 0.023f),
new QuaternionMsg(0.6944976f, 0.132371f, 0.132371f, 0.6944976f)))
},
new List <PolygonStampedMsg>(), new List <ushort>()
{
1
}, new List <KeyValueMsg>(), new List <string>(), new List <SceneLabelMsg>()),
new ProgramItemMsg(3, 4, 0, "GetReady", "", new List <string>(), new List <PoseStampedMsg>(),
new List <PolygonStampedMsg>(), new List <ushort>(), new List <KeyValueMsg>(), new List <string>(), new List <SceneLabelMsg>()),
new ProgramItemMsg(4, 1, 0, "WaitUntilUserFinishes", "", new List <string>(), new List <PoseStampedMsg>(),
new List <PolygonStampedMsg>(), new List <ushort>()
{
2
}, new List <KeyValueMsg>(), new List <string>(), new List <SceneLabelMsg>())
}, 1, 0)
});
}
// 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;
}
}
}
}
//callback which calls when service is received
public static void ServiceCallBack(string service, string yaml)
{
Debug.Log("SERVICE CALLBACK " + service);
if (service.Equals(programGetService))
{
JSONNode node = JSONNode.Parse(yaml);
ProgramMsg programMsg = new ProgramMsg(node["program"]);
//Debug.Log(programMsg.ToYAMLString());
ProgramManager.Instance.SetProgramMsgFromROS(programMsg);
ProgramHelper.SetProgramMsgFromROS(programMsg);
}
//1. moznost jak ziskat velikosti objektu .. asi se nebude pouzivat
if (service.Equals(objectGetService))
{
JSONNode node = JSONNode.Parse(yaml);
ObjectTypeMsg objectTypeMsg = new ObjectTypeMsg(node["object_type"]);
ObjectsManager.Instance.SetObjectTypeMsgFromROS(objectTypeMsg);
//Debug.Log(objectTypeMsg.ToYAMLString());
}
if (service.Equals(getAllObjectTypesService))
{
JSONNode node = JSONNode.Parse(yaml);
ObjectsManager.Instance.SetObjectTypesFromROS(node);
}
if (service.Equals(rosparamGetService))
{
JSONNode node = JSONNode.Parse(yaml);
try {
JSONNode parsed = JSONNode.Parse(node["message"]);
Debug.Log(parsed);
if (parsed["arms"] != null)
{
RobotHelper.SetRobotRadiusParam(parsed["arms"]);
}
if (parsed["locale"] != null)
{
TextToSpeechManager.Instance.SetLanguage(parsed["locale"]);
Debug.Log(parsed["locale"]);
}
}
catch (NullReferenceException e) {
Debug.Log(e);
}
}
//if (service.Equals(addCollisionPrimitiveService)) {
// JSONNode node = JSONNode.Parse(yaml);
// //Debug.Log(node);
//}
//if(service.Equals(deleteCollisionPrimitiveService)) {
// JSONNode node = JSONNode.Parse(yaml);
// //Debug.Log(node);
//}
//if (service.Equals(saveAllCollisionPrimitiveService)) {
// JSONNode node = JSONNode.Parse(yaml);
// //Debug.Log(node);
//}
//if (service.Equals(clearAllCollisionPrimitiveService)) {
// JSONNode node = JSONNode.Parse(yaml);
// //Debug.Log(node);
//}
//if (service.Equals(reloadAllCollisionPrimitiveService)) {
// JSONNode node = JSONNode.Parse(yaml);
// //Debug.Log(node);
//}
//if (service.Equals(robotLookAtLeftFeederService)) {
// JSONNode node = JSONNode.Parse(yaml);
// //Debug.Log(node);
//}
//if (service.Equals(robotLookAtRightFeederService)) {
// JSONNode node = JSONNode.Parse(yaml);
// //Debug.Log(node);
//}
}
