/// <summary>
/// Create default service state
/// </summary>
pxsonar.SonarState CreateDefaultState()
{
pxsonar.SonarState temp_state = new pxsonar.SonarState();
// Allocation for Arcos Sensor readings.
temp_state.DistanceMeasurements = new double[SonarArrayLength];
formerDistanceMeasurements = new double[SonarArrayLength];
temp_state.MaxDistance = 4000;
temp_state.HardwareIdentifier = 0;
temp_state.TimeStamp = DateTime.Now;
// Initialize Sonar readings.
for (int i = 0; i < SonarArrayLength; i++)
{
temp_state.DistanceMeasurements[i] = 0.0;
formerDistanceMeasurements[i] = 0.0;
}
// Raul - P3DX front ring sonar is 180 degress. But lateral transducers are
// Raul - centered at 90 degrees, so I consider: 196 degrees.
// Raul - I considered the aperture of one transducer is 16 degrees.
temp_state.AngularRange = 196; // 180 plus two halfs of lateral transducers.
// Raul - Angle increment for sonar transducer rays.
temp_state.AngularResolution = 1.0; // let's generate one ray per degree.
// Save state in case it doesn't exist.
SaveState(temp_state);
return(temp_state);
}
protected override void Start()
{
//configure default state
if (_state == null)
{
_state = new sonar.SonarState();
_state.HardwareIdentifier = 4;
_state.MaxDistance = 2.5; //250 cm
_state.AngularRange = 30; //degrees
SaveState(_state);
}
// Listen for each of my handlers
ActivateDsspOperationHandlers();
// Publish the service to the local Node Directory
DirectoryInsert();
// display HTTP service Uri
LogInfo(LogGroups.Console, "Service uri: ");
// Subscribe to NXT for sonar notifications
if (ValidState(_state))
{
SubscribeToNXT();
}
}
protected override void Start()
{
_physicsEngine = PhysicsEngine.GlobalInstance;
_notificationTarget = new SimulationEnginePort();
// PartnerType.Service is the entity instance name.
SimulationEngine.GlobalInstancePort.Subscribe(ServiceInfo.PartnerList, _notificationTarget);
LogInfo("Starting Simulated Sonar");
// Raul - If state cannot be read from file create a default one
if (_state == null)
{
_state = CreateDefaultState();
}
else // Use the state saved in the file, but don't forget to allocate memory for arrays.
{
_state.DistanceMeasurements = new double[SonarArrayLength];
formerDistanceMeasurements = new double[SonarArrayLength];
}
// dont start listening to DSSP operations, other than drop, until notification of entity
Activate(new Interleave(
new TeardownReceiverGroup
(
Arbiter.Receive <InsertSimulationEntity>(false, _notificationTarget, InsertEntityNotificationHandlerFirstTime),
Arbiter.Receive <dssp.DsspDefaultDrop>(false, _mainPort, DefaultDropHandler)
),
new ExclusiveReceiverGroup(),
new ConcurrentReceiverGroup()
));
// Publish the service to the local Node Directory
// DirectoryInsert();
}
private static bool ValidState(sonar.SonarState state)
{
if (state != null)
{
if (state.HardwareIdentifier >= 1 && state.HardwareIdentifier <= 4)
{
return(true);
}
}
return(false);
}
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;
}
IEnumerator <ITask> UpdateSonar()
{
var sensorOrFault = _simulatedSonarServicePort.Get();
yield return(sensorOrFault.Choice());
if (!HasError(sensorOrFault))
{
sonar.SonarState sensorState = (sonar.SonarState)sensorOrFault;
WinFormsServicePort.Post(new FormInvoke(() =>
{
_imageProcessingForm.SetSonarReadingValue(sensorState.DistanceMeasurement);
}));
}
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;
}
public virtual IEnumerator<ITask> ReplaceHandler(pxsonar.Replace replace)
{
_state = replace.Body;
replace.ResponsePort.Post(DefaultReplaceResponseType.Instance);
yield break;
}
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;
}
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);
}
}
/// <summary>
/// Create default service state
/// </summary>
pxsonar.SonarState CreateDefaultState()
{
pxsonar.SonarState temp_state = new pxsonar.SonarState();
// Allocation for Arcos Sensor readings.
temp_state.DistanceMeasurements = new double[SonarArrayLength];
formerDistanceMeasurements = new double[SonarArrayLength];
temp_state.MaxDistance = 4000;
temp_state.HardwareIdentifier = 0;
temp_state.TimeStamp = DateTime.Now;
// Initialize Sonar readings.
for (int i = 0; i < SonarArrayLength; i++)
{
temp_state.DistanceMeasurements[i] = 0.0;
formerDistanceMeasurements[i] = 0.0;
}
// Raul - P3DX front ring sonar is 180 degress. But lateral transducers are
// Raul - centered at 90 degrees, so I consider: 196 degrees.
// Raul - I considered the aperture of one transducer is 16 degrees.
temp_state.AngularRange = 196; // 180 plus two halfs of lateral transducers.
// Raul - Angle increment for sonar transducer rays.
temp_state.AngularResolution = 1.0; // let's generate one ray per degree.
// Save state in case it doesn't exist.
SaveState(temp_state);
return temp_state;
}
protected override void Start()
{
_physicsEngine = PhysicsEngine.GlobalInstance;
_notificationTarget = new SimulationEnginePort();
// PartnerType.Service is the entity instance name.
SimulationEngine.GlobalInstancePort.Subscribe(ServiceInfo.PartnerList, _notificationTarget);
LogInfo("Starting Simulated Sonar");
// Raul - If state cannot be read from file create a default one
if (_state == null)
{
_state = CreateDefaultState();
}
else // Use the state saved in the file, but don't forget to allocate memory for arrays.
{
_state.DistanceMeasurements = new double[SonarArrayLength];
formerDistanceMeasurements = new double[SonarArrayLength];
}
// dont start listening to DSSP operations, other than drop, until notification of entity
Activate(new Interleave(
new TeardownReceiverGroup
(
Arbiter.Receive<InsertSimulationEntity>(false, _notificationTarget, InsertEntityNotificationHandlerFirstTime),
Arbiter.Receive<dssp.DsspDefaultDrop>(false, _mainPort, DefaultDropHandler)
),
new ExclusiveReceiverGroup(),
new ConcurrentReceiverGroup()
));
// Publish the service to the local Node Directory
// DirectoryInsert();
}
public virtual IEnumerator <ITask> ReplaceHandler(pxsonar.Replace replace)
{
_state = replace.Body;
replace.ResponsePort.Post(DefaultReplaceResponseType.Instance);
yield break;
}
// Raul - Find best composite for Sonar Data.
/// <summary>
/// Finds the best free corridor (maximum free space ahead) in a 360 degree scan.
/// </summary>
/// <param name="south">the backward half of the scan</param>
/// <param name="north">the forward half of the scan</param>
/// <returns>best heading in degrees</returns>
private int FindBestComposite(pxsonar.SonarState south, pxsonar.SonarState north)
{
pxsonar.SonarState composite = new pxsonar.SonarState();
// Raul - I am assuming just one 8 transducer frontal sonar ring.
// Raul - That means just 8 measuments per 180 degrees.
// Raul - Therefore a 360 scan has SonarTranducers * 2 measurements.
composite.DistanceMeasurements = new double[_state.SonarTransducers*2];
for (int i = 0; i < _state.SonarTransducers*2; i++)
{
if (i < _state.SonarTransducers)
{
composite.DistanceMeasurements[i] = south.DistanceMeasurements[i];
}
else
{
composite.DistanceMeasurements[i] = north.DistanceMeasurements[i-_state.SonarTransducers];
}
}
composite.AngularResolution = 22.5;
composite.AngularRange = 360;
// composite.Units = north.Units;
return FindBestFrom(composite, 0, 0, CorridorWidthMoving);
}
public SonarUpdate(pxsonar.SonarState body)
: base(body)
{
}
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);
}
}
