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