// Update is called once per frame public void UpdatePlacePose(Vector3 placePosition, Quaternion placeOrientation) { if (ROSUnityCoordSystemTransformer.AlmostEqual(currentPlacePosition, placePosition, 0.001f) && ROSUnityCoordSystemTransformer.AlmostEqual(currentPlaceOrientation, placeOrientation, 0.001f)) { return; } currentPlacePosition = placePosition; currentPlaceOrientation = placeOrientation; if (learning) { TimeMsg currentTime = ROSTimeHelper.GetCurrentTime(); InterfaceStateMsg msg = new InterfaceStateMsg( "HOLOLENS", InterfaceStateMsg.SystemState.STATE_LEARNING, currentTime, interfaceStateMsg.GetProgramID(), interfaceStateMsg.GetBlockID(), new ProgramItemMsg( programItemMsg.GetID(), programItemMsg.GetOnSuccess(), programItemMsg.GetOnFailure(), programItemMsg.GetIType(), programItemMsg.GetName(), new List <string>(), new List <PoseStampedMsg>() { new PoseStampedMsg(new HeaderMsg(0, currentTime, "marker"), new PoseMsg(new PointMsg(ROSUnityCoordSystemTransformer.ConvertVector(currentPlacePosition)), new QuaternionMsg(ROSUnityCoordSystemTransformer.ConvertQuaternion(currentPlaceOrientation)))) }, new List <PolygonStampedMsg>(), programItemMsg.GetRefID(), programItemMsg.GetFlags(), programItemMsg.GetDoNotClear(), programItemMsg.GetLabels()), interfaceStateMsg.GetFlags(), true, InterfaceStateMsg.ErrorSeverity.NONE, InterfaceStateMsg.ErrorCode.ERROR_UNKNOWN); ROSCommunicationManager.Instance.ros.Publish(InterfaceStatePublisher.GetMessageTopic(), msg); } }
private void SendInterfaceStateChangeToROS(ProgramItemMsg programItemMsg, UInt16 blockID) { InterfaceStateMsg msg = new InterfaceStateMsg("PROJECTED UI", interfaceStateMsg.GetSystemState(), interfaceStateMsg.GetTimestamp(), interfaceStateMsg.GetProgramID(), blockID, programItemMsg, interfaceStateMsg.GetFlags(), interfaceStateMsg.GetEditEnabled(), interfaceStateMsg.GetErrorSeverity(), interfaceStateMsg.GetErrorCode()); ROSCommunicationManager.Instance.ros.Publish(InterfaceStatePublisher.GetMessageTopic(), msg); }
private IEnumerator SaveToROS() { robotArm.GetComponent <PR2GripperController>().SetArmActive(false); ROSActionHelper.OnLearningActionResult += OnLearningActionResult; Debug.Log("PICK FROM FEEDER SAVED"); //Save parameters for PICK_FROM_FEEDER TimeMsg currentTime = ROSTimeHelper.GetCurrentTime(); InterfaceStateMsg i_msg = new InterfaceStateMsg( "HOLOLENS", InterfaceStateMsg.SystemState.STATE_LEARNING, currentTime, interfaceStateMsg.GetProgramID(), interfaceStateMsg.GetBlockID(), new ProgramItemMsg( programItemMsg.GetID(), programItemMsg.GetOnSuccess(), programItemMsg.GetOnFailure(), programItemMsg.GetIType(), programItemMsg.GetName(), new List <string>() { objectTypeToPick }, new List <PoseStampedMsg>() { new PoseStampedMsg(new HeaderMsg(0, currentTime, "marker"), new PoseMsg(new PointMsg(ROSUnityCoordSystemTransformer.ConvertVector(robotArmPosition)), new QuaternionMsg(ROSUnityCoordSystemTransformer.ConvertQuaternion(robotArmRotation)))) }, new List <PolygonStampedMsg>(), programItemMsg.GetRefID(), programItemMsg.GetFlags(), programItemMsg.GetDoNotClear(), programItemMsg.GetLabels()), interfaceStateMsg.GetFlags(), true, InterfaceStateMsg.ErrorSeverity.NONE, InterfaceStateMsg.ErrorCode.ERROR_UNKNOWN); ROSCommunicationManager.Instance.ros.Publish(InterfaceStatePublisher.GetMessageTopic(), i_msg); yield return(new WaitForSecondsRealtime(0.1f)); //Request brain for goal DONE currentTime = ROSTimeHelper.GetCurrentTime(); currentRequestID = ROSActionHelper.GenerateUniqueGoalID((int)learning_request_goal.DONE, ROSTimeHelper.ToSec(currentTime)); LearningRequestActionGoalMsg requestMsg = new LearningRequestActionGoalMsg(new HeaderMsg(0, currentTime, ""), new GoalIDMsg(ROSTimeHelper.GetCurrentTime(), currentRequestID), new LearningRequestGoalMsg(learning_request_goal.DONE)); ROSCommunicationManager.Instance.ros.Publish(LearningRequestActionGoalPublisher.GetMessageTopic(), requestMsg); waiting_for_action_response = true; //Wait for action server response yield return(new WaitWhile(() => waiting_for_action_response == true)); Debug.Log("SERVER RESPONDED!"); yield return(new WaitForSecondsRealtime(0.1f)); //Switch to next instruction (should be PLACE_TO_POSE) //InteractiveProgrammingManager.Instance.PlaceToPoseLearningOverride = true; currentTime = ROSTimeHelper.GetCurrentTime(); i_msg = new InterfaceStateMsg( "HOLOLENS", InterfaceStateMsg.SystemState.STATE_LEARNING, currentTime, interfaceStateMsg.GetProgramID(), interfaceStateMsg.GetBlockID(), ProgramHelper.GetProgramItemById(interfaceStateMsg.GetBlockID(), programItemMsg.GetOnSuccess()), interfaceStateMsg.GetFlags(), false, InterfaceStateMsg.ErrorSeverity.NONE, InterfaceStateMsg.ErrorCode.ERROR_UNKNOWN); ROSCommunicationManager.Instance.ros.Publish(InterfaceStatePublisher.GetMessageTopic(), i_msg); yield return(new WaitForSecondsRealtime(0.1f)); //Request brain for goal GET_READY for learning PLACE_TO_POSE currentTime = ROSTimeHelper.GetCurrentTime(); currentRequestID = ROSActionHelper.GenerateUniqueGoalID((int)learning_request_goal.GET_READY_WITHOUT_ROBOT, ROSTimeHelper.ToSec(currentTime)); requestMsg = new LearningRequestActionGoalMsg(new HeaderMsg(0, currentTime, ""), new GoalIDMsg(currentTime, currentRequestID), new LearningRequestGoalMsg(learning_request_goal.GET_READY_WITHOUT_ROBOT)); ROSCommunicationManager.Instance.ros.Publish(LearningRequestActionGoalPublisher.GetMessageTopic(), requestMsg); waiting_for_action_response = true; //Wait for action server response yield return(new WaitWhile(() => waiting_for_action_response == true)); Debug.Log("SERVER RESPONDED!"); yield return(new WaitForSecondsRealtime(0.1f)); ROSActionHelper.OnLearningActionResult -= OnLearningActionResult; yield return(null); //robotArm.transform.parent = world_anchor.transform; robotArm.GetComponent <FollowTransform>().enabled = false; robotArm.GetComponent <PR2GripperController>().CloseGripperInstantly(); robotArm.GetComponent <PR2GripperController>().PlaceGripperToInit(); robotArm.GetComponent <PR2GripperController>().SetCollidersActive(true); robotArm.GetComponent <PR2GripperController>().SetArmActive(false); spatial_mapping.SetActive(true); }