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