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