예제 #1
0
    public IEnumerator SaveToROS()
    {
        ROSActionHelper.OnLearningActionResult += OnLearningActionResult;

        //save instruction to ROS
        Debug.Log("PLACE TO POSE SAVED");

        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(PlacePosition)),
                                           new QuaternionMsg(ROSUnityCoordSystemTransformer.ConvertQuaternion(PlaceOrientation))))
        },
                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);

        yield return(new WaitForSecondsRealtime(0.1f));

        currentTime      = ROSTimeHelper.GetCurrentTime();
        currentRequestID = ROSActionHelper.GenerateUniqueGoalID((int)learning_request_goal.DONE, ROSTimeHelper.ToSec(currentTime));
        LearningRequestActionGoalMsg requestMsg = new LearningRequestActionGoalMsg(new HeaderMsg(0, currentTime, ""),
                                                                                   new GoalIDMsg(currentTime, 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));

        ROSActionHelper.OnLearningActionResult -= OnLearningActionResult;


        //if(InteractiveProgrammingManager.Instance.followedLearningPlacePoseOverride) {
        //    InteractiveProgrammingManager.Instance.followedLearningPlacePoseOverride = false;
        //    InteractiveProgrammingManager.Instance.CurrentState = InteractiveProgrammingManager.ProgrammingManagerState.place_to_pose_vis;
        //    Visualize();
        //}

        yield return(null);
    }
예제 #2
0
    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);
    }