public IEnumerator <ITask> ReplaceHandler(pxsonar.Replace replace)
        {
            _state = replace.Body;
            if (replace.ResponsePort != null)
            {
                replace.ResponsePort.Post(dssp.DefaultReplaceResponseType.Instance);
            }

            // issue notification
            _subMgrPort.Post(new submgr.Submit(_state, dssp.DsspActions.ReplaceRequest));
            yield break;
        }
Beispiel #2
0
        public IEnumerator <ITask> ReplaceHandler(sonar.Replace replace)
        {
            if (_subscribed)
            {
                throw new Exception("Already subscribed");
            }
            else if (ValidState(replace.Body))
            {
                _state = replace.Body;
                SaveState(_state);
                replace.ResponsePort.Post(DefaultReplaceResponseType.Instance);
                SubscribeToNXT();
            }
            else
            {
                throw new Exception("Invalid State");
            }

            yield break;
        }
        private void RaycastResultsHandler(RaycastResult result)
        {
            // we just receive ray cast information from physics. Currently we just use
            // the distance measurement for each impact point reported. However, our simulation
            // engine also provides you the material properties so you can decide here to simulate
            // scattering, reflections, noise etc.
            
            // Raul - Note the closest intersection.
            double minDistance = SONAR_RANGE; 

            // Raul - The P3DX Frontal Sonar Ring have 8 SONAR transducers:
            // Raul - From left to right: S7, S6, S5, S4, S3, S2, S1, and S0.
            // Raul - Each one has an aperture of 15 degress, and their orientations are:
            // Raul - -90, -50, -30, -10, +10, +30, +50, +90 degrees respectively.
 
            // Raul - Get all distance measurements
            double[] distances = new double[SonarArrayLength];
            for (int i=0; i < SonarArrayLength; i++)
            {
                distances[i] = SONAR_RANGE;
            }

            // Raul - This code obviously needs to be rewritten:
            foreach (RaycastImpactPoint pt in result.ImpactPoints)
            {
                // LogInfo("Point:" + pt.ReadingIndex + " v:" + pt.Position.W);
                // Rays corresponding to transducer S7
                if (pt.ReadingIndex >= 0 && pt.ReadingIndex <= 15)
                {
                    if (distances[7] > pt.Position.W * 1000f)
                    {
                        // Get closest intersection in S7
                        distances[7] = (float)pt.Position.W * 1000f;
                    }
                }

                // Rays corresponding to transducer S6
                if (pt.ReadingIndex >= 40 && pt.ReadingIndex <= 55)
                {
                    // the distance to the impact has been pre-calculted from the origin 
                    // and it's in the fourth element of the vector
                    if (distances[6] > pt.Position.W * 1000f)
                    {
                        // Get closest intersection in S6
                        distances[6] = (float)pt.Position.W * 1000f;
                    }
                }

                // Rays corresponding to transducer S5
                if (pt.ReadingIndex >= 60 && pt.ReadingIndex <= 75)
                {
                    // the distance to the impact has been pre-calculted from the origin 
                    // and it's in the fourth element of the vector
                    if (distances[5] > pt.Position.W * 1000f)
                    {
                        // Get closest intersection in S5
                        distances[5] = (float)pt.Position.W * 1000f;
                    }
                }

                // Rays corresponding to transducer S4
                if (pt.ReadingIndex >= 80 && pt.ReadingIndex <= 95)
                {
                    // the distance to the impact has been pre-calculted from the origin 
                    // and it's in the fourth element of the vector
                    if (distances[4] > pt.Position.W * 1000f)
                    {
                        // Get closest intersection in S4
                        distances[4] = (float)pt.Position.W * 1000f;
                    }
                }

                // Rays corresponding to transducer S3
                if (pt.ReadingIndex >= 100 && pt.ReadingIndex <= 115)
                {
                    // the distance to the impact has been pre-calculted from the origin 
                    // and it's in the fourth element of the vector
                    if (distances[3] > pt.Position.W * 1000f)
                    {
                        // Get closest intersection in S3
                        distances[3] = (float)pt.Position.W * 1000f;
                    }
                }

                // Rays corresponding to transducer S2
                if (pt.ReadingIndex >= 120 && pt.ReadingIndex <= 135)
                {
                    // the distance to the impact has been pre-calculted from the origin 
                    // and it's in the fourth element of the vector
                    if (distances[2] > pt.Position.W * 1000f)
                    {
                        // Get closest intersection in S2
                        distances[2] = (float)pt.Position.W * 1000f;
                    }
                }

                // Rays corresponding to transducer S1
                if (pt.ReadingIndex >= 140 && pt.ReadingIndex <= 155)
                {
                    // the distance to the impact has been pre-calculted from the origin 
                    // and it's in the fourth element of the vector
                    if (distances[1] > pt.Position.W * 1000f)
                    {
                        // Get closest intersection in S1
                        distances[1] = (float)pt.Position.W * 1000f;
                    }
                }

                // Rays corresponding to transducer S0
                if (pt.ReadingIndex >= 180 && pt.ReadingIndex <= 195)
                {
                    // the distance to the impact has been pre-calculted from the origin 
                    // and it's in the fourth element of the vector
                    if (distances[0] > pt.Position.W * 1000f)
                    {
                        // Get closest intersection in S0
                        distances[0] = (float)pt.Position.W * 1000f;
                    }
                }

            }



            // Raul - Get minimum distance
            for (int i = 0; i < SonarArrayLength; i++ )
            {
                if (minDistance > distances[i])
                {
                    minDistance = distances[i];
                }
            }


            // Raul - Build Sonar State
            pxsonar.SonarState latestResults = new pxsonar.SonarState();


            // Distance between former reading and current reading is calculated
            double distance = 0.0;

            for (int i = 0; i < SonarArrayLength; i++ )
            {
                _state.DistanceMeasurements[i] = distances[i];

                // Calculate distance for each SONAR device
                distance += Math.Abs(_state.DistanceMeasurements[i] - formerDistanceMeasurements[i]);

                // Store former reading for subsequent notifications
                formerDistanceMeasurements[i] = _state.DistanceMeasurements[i];
            }

            // ONLY NOTIFY Significative chanes in sonar readings
            if (distance > SonarChangeThreshold)
            {
                // I am using SonarState.DistanceMeasurement to store the delta 
                // between last and current readings.
                _state.DistanceMeasurement = distance;


                _state.MaxDistance = minDistance; // Select the closest intersection.
                _state.AngularRange = (int)Math.Abs(_entity.RaycastProperties.EndAngle - _entity.RaycastProperties.StartAngle);
                _state.AngularResolution = _entity.RaycastProperties.AngleIncrement;
                _state.TimeStamp = DateTime.Now;

                // send replace message to self            
                pxsonar.Replace replace = new pxsonar.Replace();

                // for perf reasons dont set response port, we are just talking to ourself anyway
                replace.ResponsePort = null;
                replace.Body = _state;
                _mainPort.Post(replace);
            }
        }
Beispiel #4
0
 public virtual IEnumerator <ITask> ReplaceHandler(pxsonar.Replace replace)
 {
     _state = replace.Body;
     replace.ResponsePort.Post(DefaultReplaceResponseType.Instance);
     yield break;
 }
        private void RaycastResultsHandler(RaycastResult result)
        {
            // we just receive ray cast information from physics. Currently we just use
            // the distance measurement for each impact point reported. However, our simulation
            // engine also provides you the material properties so you can decide here to simulate
            // scattering, reflections, noise etc.

            // Raul - Note the closest intersection.
            double minDistance = SONAR_RANGE;

            // Raul - The P3DX Frontal Sonar Ring have 8 SONAR transducers:
            // Raul - From left to right: S7, S6, S5, S4, S3, S2, S1, and S0.
            // Raul - Each one has an aperture of 15 degress, and their orientations are:
            // Raul - -90, -50, -30, -10, +10, +30, +50, +90 degrees respectively.

            // Raul - Get all distance measurements
            double[] distances = new double[SonarArrayLength];
            for (int i = 0; i < SonarArrayLength; i++)
            {
                distances[i] = SONAR_RANGE;
            }

            // Raul - This code obviously needs to be rewritten:
            foreach (RaycastImpactPoint pt in result.ImpactPoints)
            {
                // LogInfo("Point:" + pt.ReadingIndex + " v:" + pt.Position.W);
                // Rays corresponding to transducer S7
                if (pt.ReadingIndex >= 0 && pt.ReadingIndex <= 15)
                {
                    if (distances[7] > pt.Position.W * 1000f)
                    {
                        // Get closest intersection in S7
                        distances[7] = (float)pt.Position.W * 1000f;
                    }
                }

                // Rays corresponding to transducer S6
                if (pt.ReadingIndex >= 40 && pt.ReadingIndex <= 55)
                {
                    // the distance to the impact has been pre-calculted from the origin
                    // and it's in the fourth element of the vector
                    if (distances[6] > pt.Position.W * 1000f)
                    {
                        // Get closest intersection in S6
                        distances[6] = (float)pt.Position.W * 1000f;
                    }
                }

                // Rays corresponding to transducer S5
                if (pt.ReadingIndex >= 60 && pt.ReadingIndex <= 75)
                {
                    // the distance to the impact has been pre-calculted from the origin
                    // and it's in the fourth element of the vector
                    if (distances[5] > pt.Position.W * 1000f)
                    {
                        // Get closest intersection in S5
                        distances[5] = (float)pt.Position.W * 1000f;
                    }
                }

                // Rays corresponding to transducer S4
                if (pt.ReadingIndex >= 80 && pt.ReadingIndex <= 95)
                {
                    // the distance to the impact has been pre-calculted from the origin
                    // and it's in the fourth element of the vector
                    if (distances[4] > pt.Position.W * 1000f)
                    {
                        // Get closest intersection in S4
                        distances[4] = (float)pt.Position.W * 1000f;
                    }
                }

                // Rays corresponding to transducer S3
                if (pt.ReadingIndex >= 100 && pt.ReadingIndex <= 115)
                {
                    // the distance to the impact has been pre-calculted from the origin
                    // and it's in the fourth element of the vector
                    if (distances[3] > pt.Position.W * 1000f)
                    {
                        // Get closest intersection in S3
                        distances[3] = (float)pt.Position.W * 1000f;
                    }
                }

                // Rays corresponding to transducer S2
                if (pt.ReadingIndex >= 120 && pt.ReadingIndex <= 135)
                {
                    // the distance to the impact has been pre-calculted from the origin
                    // and it's in the fourth element of the vector
                    if (distances[2] > pt.Position.W * 1000f)
                    {
                        // Get closest intersection in S2
                        distances[2] = (float)pt.Position.W * 1000f;
                    }
                }

                // Rays corresponding to transducer S1
                if (pt.ReadingIndex >= 140 && pt.ReadingIndex <= 155)
                {
                    // the distance to the impact has been pre-calculted from the origin
                    // and it's in the fourth element of the vector
                    if (distances[1] > pt.Position.W * 1000f)
                    {
                        // Get closest intersection in S1
                        distances[1] = (float)pt.Position.W * 1000f;
                    }
                }

                // Rays corresponding to transducer S0
                if (pt.ReadingIndex >= 180 && pt.ReadingIndex <= 195)
                {
                    // the distance to the impact has been pre-calculted from the origin
                    // and it's in the fourth element of the vector
                    if (distances[0] > pt.Position.W * 1000f)
                    {
                        // Get closest intersection in S0
                        distances[0] = (float)pt.Position.W * 1000f;
                    }
                }
            }



            // Raul - Get minimum distance
            for (int i = 0; i < SonarArrayLength; i++)
            {
                if (minDistance > distances[i])
                {
                    minDistance = distances[i];
                }
            }


            // Raul - Build Sonar State
            pxsonar.SonarState latestResults = new pxsonar.SonarState();


            // Distance between former reading and current reading is calculated
            double distance = 0.0;

            for (int i = 0; i < SonarArrayLength; i++)
            {
                _state.DistanceMeasurements[i] = distances[i];

                // Calculate distance for each SONAR device
                distance += Math.Abs(_state.DistanceMeasurements[i] - formerDistanceMeasurements[i]);

                // Store former reading for subsequent notifications
                formerDistanceMeasurements[i] = _state.DistanceMeasurements[i];
            }

            // ONLY NOTIFY Significative chanes in sonar readings
            if (distance > SonarChangeThreshold)
            {
                // I am using SonarState.DistanceMeasurement to store the delta
                // between last and current readings.
                _state.DistanceMeasurement = distance;


                _state.MaxDistance       = minDistance; // Select the closest intersection.
                _state.AngularRange      = (int)Math.Abs(_entity.RaycastProperties.EndAngle - _entity.RaycastProperties.StartAngle);
                _state.AngularResolution = _entity.RaycastProperties.AngleIncrement;
                _state.TimeStamp         = DateTime.Now;

                // send replace message to self
                pxsonar.Replace replace = new pxsonar.Replace();

                // for perf reasons dont set response port, we are just talking to ourself anyway
                replace.ResponsePort = null;
                replace.Body         = _state;
                _mainPort.Post(replace);
            }
        }