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