Пример #1
0
        private sicklrf.State _laserData = null; // not part of the state, but still accessible from all components

        #endregion Fields

        #region Methods

        /// <summary>
        /// Handles the <typeparamref name="LaserRangeFinderUpdate"/> request.
        /// </summary>
        /// <param name="update">request</param>
        protected void LaserRangeFinderUpdateHandler(LaserRangeFinderUpdate update)
        {
            //Tracer.Trace("LaserRangeFinderUpdateHandler() - Update");

            try
            {
                if (!_doSimulatedLaser)  // if simulated, ignore real data - do not call Decide()
                {
                    _laserData = (sicklrf.State)update.Body.Clone();   // laserData.DistanceMeasurements is cloned here all right

                    _state.MostRecentLaserTimeStamp = _laserData.TimeStamp;

                    updateMapperWithOdometryData();

                    if (!_mapperVicinity.robotState.ignoreLaser)        // if asked to ignore laser, we just do not update mapper with obstacles, but still call Decide() and everything else
                    {
                        updateMapperWithLaserData(_laserData);
                    }
                    else
                    {
                        lock (_mapperVicinity)
                        {
                            _mapperVicinity.computeMapPositions();
                        }
                    }

                    if (!_testBumpMode && !_state.Dropping && !_doUnitTest)
                    {
                        Decide(SensorEventSource.LaserScanning);
                    }

                    setGuiCurrentLaserData(new LaserDataSerializable() { TimeStamp = _laserData.TimeStamp.Ticks, DistanceMeasurements = (int[])_laserData.DistanceMeasurements.Clone() });
                }
            }
            catch (Exception exc)
            {
                Tracer.Trace("LaserRangeFinderUpdateHandler() - " + exc);
            }

            update.ResponsePort.Post(DefaultUpdateResponseType.Instance);
        }
Пример #2
0
        /// <summary>
        /// Handles Replace notifications from the Laser partner
        /// </summary>
        /// <remarks>Posts a <typeparamref name="LaserRangeFinderUpdate"/> to itself.</remarks>
        /// <param name="replace">notification</param>
        /// <returns>task enumerator</returns>
        IEnumerator<ITask> LaserReplaceNotification(sicklrf.Replace replace)
        {
            // When this handler is called a couple of notifications may
            // have piled up. We only want the most recent one.
            sicklrf.State laserData = GetMostRecentLaserNotification(replace.Body);

            LaserRangeFinderUpdate request = new LaserRangeFinderUpdate(laserData);

            _mainPort.Post(request);

            yield return Arbiter.Choice(
                request.ResponsePort,
                delegate(DefaultUpdateResponseType response) { },
                delegate(Fault fault) { }
            );

            // Skip messages that have been queued up in the meantime.
            // The notification that are lingering are out of data by now.
            GetMostRecentLaserNotification(laserData);

            // Reactivate the handler.
            Activate(
                Arbiter.ReceiveWithIterator<sicklrf.Replace>(false, _laserNotify, LaserReplaceNotification)
            );
        }
Пример #3
0
        /// <summary>
        /// Handles the <typeparamref name="LaserRangeFinderUpdate"/> request.
        /// </summary>
        /// <param name="update">request</param>
        void LaserRangeFinderUpdateHandler(LaserRangeFinderUpdate update)
        {
            sicklrf.State laserData = update.Body;
            _state.MostRecentLaser = laserData.TimeStamp;

            int distance = FindNearestObstacleInCorridor(laserData, CorridorWidthMapping, 45);

            // AvoidCollision and EnterOpenSpace have precedence over
            // all other state transitions and are thus handled first.
            AvoidCollision(distance);
            EnterOpenSpace(distance);

            UpdateLogicalState(laserData, distance);

            update.ResponsePort.Post(DefaultUpdateResponseType.Instance);
        }