/// <summary> /// Handler for Contact Sensor Notifications /// </summary> /// <param name="updateNotification"></param> void BumperNotificationHandler(contactsensor.Update updateNotification) { // Since we are writing a generic wander service we dont really know // which bumper is front, side, rear etc. We expect a navigation service to be tuned // to a robot platform or read configuration through its initial state. // here, we just assume the bumpers are named intuitively so we search by name contactsensor.ContactSensor s = updateNotification.Body; if (!s.Pressed) { return; } if (!string.IsNullOrEmpty(s.Name) && s.Name.ToLowerInvariant().Contains("front")) { SpawnIterator <double>(-1, BackUpTurnAndMove); return; } if (!string.IsNullOrEmpty(s.Name) && s.Name.ToLowerInvariant().Contains("rear")) { SpawnIterator <double>(1, BackUpTurnAndMove); return; } }
/// <summary> /// receive sensor data from the Srv1 /// </summary> /// <param name="replace"></param> private void SensorNotificationHandler(srv1bumper.Replace replace) { const int sensorThreshold = 100; for (int ix = 1; ix <= replace.Body.Sensors.Length; ix++) { bumper.ContactSensor bump = _state.Sensors.Find(delegate(bumper.ContactSensor s) { return(s.HardwareIdentifier == ix); }); if (bump == null) { continue; } srv1bumper.Sensor sensor = replace.Body.Sensors[ix - 1]; bool sensorPressed = (sensor.value > sensorThreshold); bool changed = (sensorPressed != bump.Pressed); if (changed) { bump.Pressed = sensorPressed; //notify subscribers on any bumper pressed or unpressed _subMgrPort.Post(new submgr.Submit(bump, DsspActions.UpdateRequest)); } } }
/// <summary> /// Service Start /// </summary> protected override void Start() { //configure default state if (_state == null) { _state = new bumper.ContactSensorArrayState(); _state.Sensors = new List<bumper.ContactSensor>(); bumper.ContactSensor leftBumper = new bumper.ContactSensor(); leftBumper.Name = "FrontLeftIR"; //note: name must contain 'front' for tutorial3 bumper.ContactSensor rightBumper = new bumper.ContactSensor(); rightBumper.Name = "FrontRightIR"; _state.Sensors.Add(leftBumper); _state.Sensors.Add(rightBumper); } // Listen on the main port for requests and call the appropriate handler. ActivateDsspOperationHandlers(); // Publish the service to the local Node Directory DirectoryInsert(); // display HTTP service Uri LogInfo(LogGroups.Console, "Service uri: "); SubscribeToScribblerBase(); }
void InsertEntityNotificationHandler(simengine.InsertSimulationEntity ins) { // _entity = (simengine.BumperArrayEntity)ins.Body; _entity = (PioneerBumperEntity)ins.Body; _entity.ServiceContract = Contract.Identifier; // reinitialize the state CreateDefaultState(); pxContactSensor.ContactSensor cs = null; // The simulation bumper service uses a simple heuristic to assign contact sensors, to physics shapes: // Half the sensors go infront, the other half in the rear. Assume front sensors are first in the list // In the for loop below we create a lookup table that matches simulation shapes with sensors. When // the physics engine notifies us with a contact, it provides the shape that came into contact. We need to // translate that to sensor. In the future we might add an object Context field to shapes to make this easier for (int i = 0; i < _entity.Shapes.Length; i++) { cs = new pxContactSensor.ContactSensor(); cs.Name = _entity.Shapes[i].State.Name; cs.HardwareIdentifier = i; _state.Sensors.Add(cs); _bumperShapeToSensorTable.Add((physics.BoxShape)_entity.Shapes[i], cs); } // subscribe to bumper simulation entity for contact notifications _entity.Subscribe(_contactNotificationPort); // Activate a handler on the notification port, it will run when contacts occur in simulation Activate(Arbiter.Receive(false, _contactNotificationPort, PhysicsContactNotificationHandler)); }
/// <summary> /// Service Start /// </summary> protected override void Start() { //configure default state if (_state == null) { _state = new bumper.ContactSensorArrayState(); _state.Sensors = new List <bumper.ContactSensor>(); bumper.ContactSensor leftBumper = new bumper.ContactSensor(); leftBumper.Name = "FrontLeftIR"; //note: name must contain 'front' for tutorial3 bumper.ContactSensor rightBumper = new bumper.ContactSensor(); rightBumper.Name = "FrontRightIR"; _state.Sensors.Add(leftBumper); _state.Sensors.Add(rightBumper); } // Listen on the main port for requests and call the appropriate handler. ActivateDsspOperationHandlers(); // Publish the service to the local Node Directory DirectoryInsert(); // display HTTP service Uri LogInfo(LogGroups.Console, "Service uri: "); SubscribeToScribblerBase(); }
public IEnumerator <ITask> UpdateHandler(bumper.Update update) { bumper.ContactSensor sensor = _state.Sensors.Find( delegate(bumper.ContactSensor s) { return(update.Body.HardwareIdentifier == s.HardwareIdentifier); }); if (sensor != null) { sensor.Name = update.Body.Name; sensor.Pressed = update.Body.Pressed; sensor.TimeStamp = update.Body.TimeStamp; update.ResponsePort.Post(DefaultUpdateResponseType.Instance); } throw new InvalidOperationException("Contact Sensor " + update.Body.HardwareIdentifier + " : " + update.Body.Name + " does not exist."); }
void BumperNotificationHandler(contactsensor.Update updateNotification) { contactsensor.ContactSensor s = updateNotification.Body; if (!s.Pressed) { return; } if (!string.IsNullOrEmpty(s.Name) && s.Name.ToLowerInvariant().Contains("left")) { //say something speech.SayTextRequest saythis = new speech.SayTextRequest(); saythis.SpeechText = "Turning right"; _speechPort.SayText(saythis); //update state _state.CurrentAction = "Turning right"; //move SpawnIterator <int>(-800, BackUpTurnAndMove); } else if (!string.IsNullOrEmpty(s.Name) && s.Name.ToLowerInvariant().Contains("right")) { //say something speech.SayTextRequest saythis = new speech.SayTextRequest(); saythis.SpeechText = "Turning left"; _speechPort.SayText(saythis); //update state _state.CurrentAction = "Turning left"; //move SpawnIterator <int>(800, BackUpTurnAndMove); } else if (!string.IsNullOrEmpty(s.Name) && s.Name.ToLowerInvariant().Contains("stall")) { //say something speech.SayTextRequest saythis = new speech.SayTextRequest(); saythis.SpeechText = "Ouch, let go."; _speechPort.SayText(saythis); //update state _state.CurrentAction = "Turning around"; //move SpawnIterator <int>(800, BackUpTurnAndMove); } }
protected override void Start() { //configure initial state if (_state == null) { LogInfo("TrackRoamerBumper:Start(): _state == null - initializing..."); _state = new pxbumper.ContactSensorArrayState(); _state.Sensors = new List<pxbumper.ContactSensor>(); pxbumper.ContactSensor leftBumper = new pxbumper.ContactSensor(); leftBumper.HardwareIdentifier = 101; leftBumper.Name = "Front Whisker Left"; _state.Sensors.Add(leftBumper); pxbumper.ContactSensor rightBumper = new pxbumper.ContactSensor(); rightBumper.HardwareIdentifier = 201; rightBumper.Name = "Front Whisker Right"; _state.Sensors.Add(rightBumper); SaveState(_state); } else { LogInfo("TrackRoamerBumper:Start(): _state is supplied by file: " + _configFile); } base.Start(); MainPortInterleave.CombineWith( new Interleave( new TeardownReceiverGroup(), new ExclusiveReceiverGroup( Arbiter.Receive<powerbrick.UpdateWhiskers>(true, notificationPortWhiskers, WhiskersNotificationHandler) ), new ConcurrentReceiverGroup() ) ); // display HTTP service Uri LogInfo("TrackRoamerBumper:Start() Service URI=" + ServiceInfo.HttpUri()); // Subscribe to the Hardware Controller for bumper notifications SubscribeToTrackRoamerBot(); }
/// <summary> /// Analog Sensor Notification Handler /// </summary> /// <param name="notification"></param> private void AnalogSensorNotificationHandler(analog.Replace notification) { LogVerbose(LogGroups.Console, string.Format("Sensor Notification: {0} {1}", notification.Body.HardwareIdentifier, notification.Body.RawMeasurement)); foreach (SensorRange key in _state.RuntimeConfiguration.Keys) { if (key.HardwareIdentifier != notification.Body.HardwareIdentifier) { continue; } PortConfiguration sensorConfig = _state.RuntimeConfiguration[key]; string contactSensorName = key.ContactSensorName; int priorIx = _contactSensorArrayState.Sensors.FindIndex( delegate(bumper.ContactSensor gencs) { return(gencs.HardwareIdentifier == notification.Body.HardwareIdentifier && gencs.Name == contactSensorName); }); bool priorPressed = (priorIx < 0) ? false : _contactSensorArrayState.Sensors[priorIx].Pressed; // Send a ContactSensor notification here. bumper.ContactSensor cs = new bumper.ContactSensor(sensorConfig.HardwareIdentifier, contactSensorName); cs.Pressed = sensorConfig.Pressed(notification.Body.RawMeasurement); sensorConfig.Contact = cs.Pressed; if (priorIx < 0) { _contactSensorArrayState.Sensors.Add(cs); } if (priorIx < 0 || priorPressed != cs.Pressed) { if (priorIx >= 0) { _contactSensorArrayState.Sensors[priorIx].Pressed = cs.Pressed; } SendNotification <bumper.Update>(_subMgrPort, new bumper.Update(cs)); } } }
protected override void Start() { //configure initial state if (_state == null) { _state = new bumper.ContactSensorArrayState(); _state.Sensors = new List <contactsensorbase.ContactSensor>(); contactsensorbase.ContactSensor defaultBumper = new contactsensorbase.ContactSensor(); defaultBumper.HardwareIdentifier = 1; defaultBumper.Name = "Default Bumper 1"; _state.Sensors.Add(defaultBumper); SaveState(_state); } Port <bool> successPort = new Port <bool>(); SpawnIterator(successPort, ConfigureHardware); Activate(Arbiter.Receive(false, successPort, delegate(bool success) { if (!success) { LogError("Surveyor SRV-1 Service failed to start. Shutting down."); Shutdown(); } } )); // Listen on the main port for requests and call the appropriate handler. ActivateDsspOperationHandlers(); // Publish the service to the local Node Directory DirectoryInsert(); // display HTTP service Uri LogInfo(LogGroups.Console, "Service uri: "); }
/// <summary> /// Service Start /// </summary> protected override void Start() { //configure default state if (_state == null) { _state = new bumper.ContactSensorArrayState(); _state.Sensors = new List <bumper.ContactSensor>(); //A note on these names: //the bumper names must contain 'front' for RoboticsTutorial3 to work //the bumpers should be labeled left and right for the Scribbler Wander service to work //also for the SensorNotificationHandler below //If multiple words, should be spaced properly so TTS can say it right bumper.ContactSensor leftBumper = new bumper.ContactSensor(); leftBumper.Name = "Front Left"; bumper.ContactSensor rightBumper = new bumper.ContactSensor(); rightBumper.Name = "Front Right"; bumper.ContactSensor stallSensor = new bumper.ContactSensor(); stallSensor.Name = "Stall"; _state.Sensors.Add(leftBumper); _state.Sensors.Add(rightBumper); _state.Sensors.Add(stallSensor); } // Listen on the main port for requests and call the appropriate handler. ActivateDsspOperationHandlers(); // Publish the service to the local Node Directory DirectoryInsert(); // display HTTP service Uri LogInfo(LogGroups.Console, "Service uri: "); SubscribeToScribblerBase(); }
public BumperUpdate(bumper.ContactSensor body) : base(body) { }
/// <summary> /// Get state from LEGO and set up a contact sensor /// for each of the configured ports. /// </summary> /// <param name="resultPort"></param> /// <returns></returns> private IEnumerator <ITask> GetLegoBumperConfiguration(Port <LegoNxtBumperState> resultPort) { LegoNxtBumperState configArray = new LegoNxtBumperState(); nxt.LegoNxtState legoState = null; bool existingBumperConfig = (_bumperConfigState != null && _bumperConfigState.BumperConfigList != null && _bumperConfigState.BumperConfigList.Count > 0); bool validLegoState = false; int tryCount = 0; while (!validLegoState) { tryCount++; // See if the LEGO is configured. yield return(Arbiter.Choice(_legoPort.Get(), delegate(nxt.LegoNxtState legoNxtState) { legoState = legoNxtState; }, delegate(Fault fault) { LogError(fault); })); validLegoState = (legoState != null && legoState.BrickConfig != null && legoState.BrickConfig.SensorPort != null && legoState.BrickConfig.SensorPort.Length > 0 && legoState.ComPort > 0); // If we don't have valid configuration from the NXT Brick // wait a little and try again. if (!validLegoState) { LogVerbose(LogGroups.Console, "Waiting for LEGO NXT to be initialized"); System.Threading.Thread.Sleep(500); } } configArray.ContactSensorArrayState = new bumper.ContactSensorArrayState(); configArray.ContactSensorArrayState.Sensors = new List <bumper.ContactSensor>(); configArray.BumperConfigList = new List <LegoNxtBumperConfig>(); // We do not have a valid configuration from the NXT. if (!validLegoState) { // Do we have a valid prior configuration? if (existingBumperConfig) { configArray.BumperConfigList = _bumperConfigState.BumperConfigList; configArray.ContactSensorArrayState = _bumperConfigState.ContactSensorArrayState; } else { // We have no prior configuration LegoNxtBumperConfig bumperConfig = new LegoNxtBumperConfig(); bumperConfig.HardwareIdentifier = 1; bumperConfig.SensorType = nxt.SensorDefinition.SensorType.Touch; bumperConfig.Name = "Default Bumper 1"; bumperConfig.ThresholdLow = 1; bumperConfig.ThresholdHigh = 1; configArray.BumperConfigList.Add(bumperConfig); bumper.ContactSensor defaultBumper = new bumper.ContactSensor(); defaultBumper.HardwareIdentifier = bumperConfig.HardwareIdentifier; defaultBumper.Name = bumperConfig.Name; configArray.ContactSensorArrayState.Sensors.Add(defaultBumper); } } else // We have valid LEGO NXT Configuration { int hardwareIdentifier = 1; foreach (nxt.SensorConfig sensorConfig in legoState.BrickConfig.SensorPort) { LegoNxtBumperConfig bumperConfig = new LegoNxtBumperConfig(); bumperConfig.HardwareIdentifier = hardwareIdentifier; bumperConfig.SensorType = sensorConfig.Type; if (sensorConfig.Type == nxt.SensorDefinition.SensorType.Touch) { bumperConfig.Name = "Touch Bumper " + hardwareIdentifier.ToString(); bumperConfig.ThresholdLow = 1; bumperConfig.ThresholdHigh = 1; } else if (sensorConfig.Type == nxt.SensorDefinition.SensorType.Sonar) { bumperConfig.Name = "Sonar Bumper " + hardwareIdentifier.ToString(); if (sensorConfig.ExternalRange) { bumperConfig.ThresholdLow = sensorConfig.LowThresh; bumperConfig.ThresholdHigh = sensorConfig.HighThresh; } else { bumperConfig.ThresholdLow = 0; bumperConfig.ThresholdHigh = 15; } } // Sound or Light with an external range for a trigger? else if (sensorConfig.ExternalRange && (0 != (sensorConfig.Type & (nxt.SensorDefinition.SensorType.LightOn | nxt.SensorDefinition.SensorType.LightOff | nxt.SensorDefinition.SensorType.Sound)))) { bumperConfig.Name = sensorConfig.Type.ToString() + " Sensor " + hardwareIdentifier.ToString(); bumperConfig.ThresholdLow = sensorConfig.LowThresh; bumperConfig.ThresholdHigh = sensorConfig.HighThresh; } #region Merge with prior config // Look to see if this sensor was already configured if (existingBumperConfig) { LegoNxtBumperConfig prevBumperConfig = _bumperConfigState.BumperConfigList.Find(delegate(LegoNxtBumperConfig cfg) { return(cfg.HardwareIdentifier == hardwareIdentifier); }); if (prevBumperConfig != null) { // yes, use the old name. bumperConfig.Name = prevBumperConfig.Name; // if the sensor type has not changed, use the old threshold values. if (prevBumperConfig.SensorType == bumperConfig.SensorType) { bumperConfig.ThresholdLow = prevBumperConfig.ThresholdLow; bumperConfig.ThresholdHigh = prevBumperConfig.ThresholdHigh; } } } #endregion if (!string.IsNullOrEmpty(bumperConfig.Name)) { configArray.BumperConfigList.Add(bumperConfig); configArray.ContactSensorArrayState.Sensors.Add(new bumper.ContactSensor(hardwareIdentifier, bumperConfig.Name)); } hardwareIdentifier++; } } resultPort.Post(configArray); yield break; }
/// <summary> /// Analog Sensor Notification Handler /// </summary> /// <param name="notification"></param> private void AnalogSensorNotificationHandler(analog.Replace notification) { LogVerbose(LogGroups.Console, string.Format("Sensor Notification: {0} {1}", notification.Body.HardwareIdentifier, notification.Body.RawMeasurement)); foreach (SensorRange key in _state.RuntimeConfiguration.Keys) { if (key.HardwareIdentifier != notification.Body.HardwareIdentifier) continue; PortConfiguration sensorConfig = _state.RuntimeConfiguration[key]; string contactSensorName = key.ContactSensorName; int priorIx = _contactSensorArrayState.Sensors.FindIndex( delegate(bumper.ContactSensor gencs) { return gencs.HardwareIdentifier == notification.Body.HardwareIdentifier && gencs.Name == contactSensorName; }); bool priorPressed = (priorIx < 0) ? false : _contactSensorArrayState.Sensors[priorIx].Pressed; // Send a ContactSensor notification here. bumper.ContactSensor cs = new bumper.ContactSensor(sensorConfig.HardwareIdentifier, contactSensorName); cs.Pressed = sensorConfig.Pressed(notification.Body.RawMeasurement); sensorConfig.Contact = cs.Pressed; if (priorIx < 0) _contactSensorArrayState.Sensors.Add(cs); if (priorIx < 0 || priorPressed != cs.Pressed) { if (priorIx >= 0) _contactSensorArrayState.Sensors[priorIx].Pressed = cs.Pressed; SendNotification<bumper.Update>(_subMgrPort, new bumper.Update(cs)); } } }