Ejemplo n.º 1
0
        public async Task <bool> AlignToArTag(int dictionary, double size, int tagId, Simple2DPose goalTagPose)
        {
            _abort = false;
            _tagId = tagId;
            _skillHelper.LogMessage($"Starting AR tag alignment. Desired tag pose is [x, y, yaw] {goalTagPose.X:f3}, {goalTagPose.Y:f3}, {goalTagPose.Yaw.Degrees():f0}].");

            // Look straight ahead
            await _skillHelper.MoveHeadAsync(0, 0, 0);

            // Startup AR tag detection
            _misty.EnableCameraService(OnResponse);
            _misty.EnableCameraService(OnResponse);                  // Calling twice to make sure. TODO: something smarter
            _misty.StartArTagDetector(dictionary, size, OnResponse);
            _misty.StartArTagDetector(dictionary, size, OnResponse); // Calling twice to make sure. TODO: something smarter
            _misty.RegisterArTagDetectionEvent(ArTagCallback, 100, true, "artagevent", OnResponse);

            // Wait for measurments to start.
            if (!await WaitForMeasurementAsync(10, 2))
            {
                if (_abort)
                {
                    return(false);
                }

                // Never detected the AR tag. Try a small sweep.
                _skillHelper.MistySpeak("I can't see the tag. Looking around for it.");
                _skillHelper.LogMessage("Do not see the tag. Starting sweep.");
                bool arTagFound = false;
                await _skillHelper.TurnAsync(20);

                int maxTurns = 5;
                while (!arTagFound && --maxTurns >= 0)
                {
                    arTagFound = await WaitForMeasurementAsync(1, 1);

                    if (_abort)
                    {
                        return(false);
                    }
                    if (!arTagFound)
                    {
                        await _skillHelper.TurnAsync(-10);
                    }
                }

                if (!arTagFound)
                {
                    _skillHelper.MistySpeak("I cannot find the tag.");
                    _skillHelper.LogMessage("Never detected the AR tag.");
                    Cleanup();
                    return(false);
                }
            }

            _skillHelper.LogMessage($"AR tag located at [x, y, yaw] : [{_currentTagPose.X:f3}, {_currentTagPose.Y:f3}, {_currentTagPose.Yaw.Degrees():f1}]. " +
                                    $"Offset : [{(_currentTagPose.X - goalTagPose.X):f3}, {(_currentTagPose.Y - goalTagPose.Y):f3}, {(_currentTagPose.Yaw.Degrees() - goalTagPose.Yaw.Degrees()):f1}].");

            if (Math.Abs(_currentTagPose.X - goalTagPose.X) < X_TOLERANCE && Math.Abs(_currentTagPose.Y - goalTagPose.Y) < Y_TOLERANCE &&
                Math.Abs(_currentTagPose.Yaw.Degrees() - goalTagPose.Yaw.Degrees()) < YAW_TOLERANCE)
            {
                _skillHelper.MistySpeak("We're already aligned with the tag. No need to move.");
                _skillHelper.LogMessage("No movement needed to align to tag.");
            }
            else
            {
                int retries = 0;
                while (Math.Abs(_currentTagPose.X - goalTagPose.X) > X_TOLERANCE || Math.Abs(_currentTagPose.Y - goalTagPose.Y) > Y_TOLERANCE ||
                       Math.Abs(_currentTagPose.Yaw.Degrees() - goalTagPose.Yaw.Degrees()) > YAW_TOLERANCE)
                {
                    MoveSequence moveSequence = NavigationHelper.CalculateMoveSequence(_currentTagPose, goalTagPose);
                    _skillHelper.LogMessage($"Movement sequence to goal is turn {moveSequence.Turn1.Degrees():f0} degrees, drive {moveSequence.DriveDistance:f3} meters, " +
                                            $"and turn {moveSequence.Turn2.Degrees():f0} degrees.");

                    await _skillHelper.TurnAsync(moveSequence.Turn1.Degrees());

                    if (_abort)
                    {
                        return(false);
                    }

                    await _skillHelper.DriveAsync(moveSequence.DriveDistance, true);

                    if (_abort)
                    {
                        return(false);
                    }

                    await _skillHelper.TurnAsync(moveSequence.Turn2.Degrees());

                    if (_abort)
                    {
                        return(false);
                    }

                    _skillHelper.LogMessage($"AR tag located at [x, y, yaw] : [{_currentTagPose.X:f3}, {_currentTagPose.Y:f3}, {_currentTagPose.Yaw.Degrees():f1}]. " +
                                            $"Offset : [{(_currentTagPose.X - goalTagPose.X):f3}, {(_currentTagPose.Y - goalTagPose.Y):f3}, {(_currentTagPose.Yaw.Degrees() - goalTagPose.Yaw.Degrees()):f1}].");

                    if (retries++ > 5)
                    {
                        _skillHelper.LogMessage("Failed to line up with the tag.");
                        _skillHelper.MistySpeak("I can't seem to line up right. I give up.");
                        await Task.Delay(5000);

                        Cleanup();
                        return(false);
                    }
                }

                _skillHelper.MistySpeak("Alignment complete.");
            }

            Cleanup();

            return(true);
        }