Beispiel #1
0
        public async Task <bool> StartTrackingAsync(string mapName)
        {
            _skillHelper.LogMessage($"Attempting to track within map {mapName}.");
            _abort = false;

            // We don't get the newly loaded map back from Occ software until we start and stop tracking. So...
            _skillHelper.LogMessage("Loading map and starting tracking.");
            _misty.SetCurrentSlamMap(mapName, OnResponse);
            await Task.Delay(1000);

            _misty.StartTracking(OnResponse);

            await _skillHelper.MoveHeadAsync(_headPitchOffset, _headRollOffset, _headYawOffset);

            _misty.RegisterSlamStatusEvent(SlamStatusCallback, 0, true, "MapDockSlamStatusEvent", null, OnResponse);
            _misty.RegisterSelfStateEvent(SelfStateCallback, 250, true, "MapDockSelfStateEvent", OnResponse);

            // There is a current defect where switching maps does not fully take effect until we start tracking
            // and stop tracking. So we need to start, stop, and re-start.
            _misty.StopTracking(OnResponse);
            await Task.Delay(4000);

            _misty.StartTracking(OnResponse);
            await Task.Delay(4000);

            _misty.GetMap(OnResponse);
            await Task.Delay(4000);

            if (_slamStatus == null || _slamStatus.SensorStatus != MistyRobotics.Common.Types.SlamSensorMode.Streaming)
            {
                _skillHelper.LogMessage("Failed to start tracking.");
                Cleanup();
                return(false);
            }

            _skillHelper.LogMessage("Checking for pose.");
            int count = 0;

            while (_slamStatus.RunMode != MistyRobotics.Common.Types.SlamRunningMode.Tracking && count++ < 40)
            {
                await _skillHelper.TurnAsync(10);

                _misty.GetSlamStatus(OnResponse);
            }

            if (_slamStatus.RunMode == MistyRobotics.Common.Types.SlamRunningMode.Tracking)
            {
                _skillHelper.LogMessage($"Pose acquired. Map cell is [{_mapCell.X},{_mapCell.Y}]. Map yaw is {_mapYaw:f2}.");
                _tracking = true;
            }
            else
            {
                _skillHelper.LogMessage("Unable to obtain pose.");
                Cleanup();
                _tracking = false;
            }

            return(_tracking);
        }