Beispiel #1
0
    private static void GetArmsPosition()
    {
        TimeMsg currentTime             = ROSTimeHelper.GetCurrentTime();
        TFSubscriptionActionGoalMsg msg = new TFSubscriptionActionGoalMsg(new HeaderMsg(1, currentTime, ""),
                                                                          new GoalIDMsg(currentTime, ROSActionHelper.GenerateUniqueGoalID("robot_radius", ROSTimeHelper.ToSec(currentTime))),
                                                                          new TFSubscriptionGoalMsg(new List <string>()
        {
            leftArm.BaseLink, rightArm.BaseLink
        }, "marker", 0f, 0f, 1f));

        ROSCommunicationManager.Instance.ros.Publish(TF2WebRepublisherGoalPublisher.GetMessageTopic(), msg);
    }
Beispiel #2
0
    // 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);
        }
    }
Beispiel #3
0
    // Update is called once per frame
    void Update()
    {
        if (SystemStarter.Instance.calibrated)
        {
            //Vector3 relativePositionToParent = gameObject.transform.InverseTransformPoint(parentGameObject.transform.position);
            Vector3    relativePositionToParent = parentGameObject.transform.InverseTransformPoint(gameObject.transform.position);
            Quaternion relativeRotationToParent = Quaternion.Inverse(parentGameObject.transform.rotation) * gameObject.transform.rotation;


            tfMsg = new TFMessageMsg(new List <TransformStampedMsg>()
            {
                new TransformStampedMsg(new HeaderMsg(0, ROSTimeHelper.GetCurrentTime(), frame_id), child_frame_id,
                                        new TransformMsg(new Vector3Msg(relativePositionToParent.x, -relativePositionToParent.y, relativePositionToParent.z),
                                                         new QuaternionMsg(-relativeRotationToParent.x, relativeRotationToParent.y, -relativeRotationToParent.z, relativeRotationToParent.w)))
            });

            ROSCommunicationManager.Instance.ros.Publish(TFPublisher.GetMessageTopic(), tfMsg, debug_log: false);
        }
    }
Beispiel #4
0
    private static void CancelTF2Action()
    {
        GoalIDMsg msg = new GoalIDMsg(ROSTimeHelper.GetCurrentTime(), "");

        ROSCommunicationManager.Instance.ros.Publish(TF2WebRepublisherCancelPublisher.GetMessageTopic(), msg);
    }
Beispiel #5
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);
    }
Beispiel #6
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);
    }
Beispiel #7
0
    private void PublishClick()
    {
        click_counter++;

        if (ROSCommunicationManager.Instance.ros._connected)
        {
            ROSCommunicationManager.Instance.ros.Publish(HoloLensClickPublisher.GetMessageTopic(), new HeaderMsg(click_counter, ROSTimeHelper.GetCurrentTime(), "hololens"));
        }
    }