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; }
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); } }
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); } }