/// <summary>
/// Initialize State
/// </summary>
private void InitializeState()
{
if (_state == null)
{
_state = new TouchSensorState();
}
if (_state.PollingFrequencyMs == 0)
{
_state.PollingFrequencyMs = Contract.DefaultPollingFrequencyMs;
}
_state.TimeStamp = DateTime.MinValue;
_state.Connected = false;
_state.TouchSensorOn = false;
if (_genericContactState == null)
{
_genericContactState = new contactsensor.ContactSensorArrayState();
}
if (_genericContactState.Sensors == null)
{
_genericContactState.Sensors = new List <contactsensor.ContactSensor>();
}
if (_genericContactState.Sensors.Count == 0)
{
_genericContactState.Sensors.Add(new contactsensor.ContactSensor(0, "Touch Sensor"));
}
}
private void ValidateState()
{
//configure initial state
if (_state == null)
{
_state = new bumper.ContactSensorArrayState();
}
if (_state.Sensors == null)
{
_state.Sensors = new List <bumper.ContactSensor>();
}
if (_state.Sensors.Count == 0)
{
_state.Sensors.Add(new bumper.ContactSensor((int)RoombaBumpers.LeftBumper, "Front Left Bumper"));
_state.Sensors.Add(new bumper.ContactSensor((int)RoombaBumpers.RightBumper, "Front Right Bumper"));
_state.Sensors.Add(new bumper.ContactSensor((int)RoombaBumpers.Wall, "Wall"));
_state.Sensors.Add(new bumper.ContactSensor((int)RoombaBumpers.VirtualWall, "Virtual Wall"));
_state.Sensors.Add(new bumper.ContactSensor((int)RoombaBumpers.CliffLeft, "Cliff Left"));
_state.Sensors.Add(new bumper.ContactSensor((int)RoombaBumpers.CliffFrontLeft, "Cliff Front Left"));
_state.Sensors.Add(new bumper.ContactSensor((int)RoombaBumpers.CliffFrontRight, "Cliff Front Right"));
_state.Sensors.Add(new bumper.ContactSensor((int)RoombaBumpers.CliffRight, "Cliff Right"));
_state.Sensors.Add(new bumper.ContactSensor((int)RoombaBumpers.WheelDropLeft, "Left Wheel Drop"));
_state.Sensors.Add(new bumper.ContactSensor((int)RoombaBumpers.WheelDropRight, "Right Wheel Drop"));
_state.Sensors.Add(new bumper.ContactSensor((int)RoombaBumpers.WheelDropRear, "Rear Wheel Drop"));
_state.Sensors.Add(new bumper.ContactSensor(99, "Wheels Stalled"));
SaveState(_state);
}
}
/// <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> ReplaceHandler(pxContactSensor.Replace replace)
{
_state = replace.Body;
replace.ResponsePort.Post(dssp.DefaultReplaceResponseType.Instance);
_subMgrPort.Post(new submgr.Submit(_state, dssp.DsspActions.ReplaceRequest));
yield break;
}
/// <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 virtual IEnumerator <ITask> RemoveDeviceConfigurationHandler(ResetConfiguration resetConfiguration)
{
// Clear Runtime Configuration
UnsubscribeAllSensors();
_state.SensorConfiguration = new List <SensorConfiguration>();
_state.RuntimeConfiguration = null;
_contactSensorArrayState = new bumper.ContactSensorArrayState();
resetConfiguration.ResponsePort.Post(DefaultDeleteResponseType.Instance);
yield break;
}
/// <summary>
/// Checks whether at least one of the contact sensors is pressed.
/// </summary>
/// <param name="bumpers"><code>true</code> if at least one bumper in <paramref name="bumpers"/> is pressed, otherwise <code>false</code></param>
/// <returns></returns>
private bool BumpersPressed(bumper.ContactSensorArrayState bumpers)
{
if (bumpers.Sensors == null)
{
return(false);
}
foreach (bumper.ContactSensor s in bumpers.Sensors)
{
if (s.Pressed)
{
return(true);
}
}
return(false);
}
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>
/// Inspect the ContactSensorArrayState to make sure it
/// exists and that each sensor is associated with a valid
/// LEGO sensor port (1-4).
/// </summary>
/// <param name="state"></param>
/// <returns></returns>
private static bool ValidState(bumper.ContactSensorArrayState state)
{
if (state != null)
{
if (state.Sensors != null)
{
foreach (bumper.ContactSensor sensor in state.Sensors)
{
if (sensor.HardwareIdentifier < 1 || sensor.HardwareIdentifier > 4)
{
return(false);
}
}
return(true);
}
}
return(false);
}
/// <summary>
/// Validate the State.
/// </summary>
private void ValidateState()
{
bool saveState = false;
if (_state == null)
{
saveState = true;
_state = new NxtContactSensorArrayState();
}
if (_state.SensorConfiguration == null)
{
saveState = true;
_state.SensorConfiguration = new List <SensorConfiguration>();
}
if (_state.SensorConfiguration.Count == 0)
{
saveState = true;
_state.SensorConfiguration.Add(new SensorConfiguration("Pressed", touch.Contract.DeviceModel, string.Empty, 1.0, 1.0));
_state.SensorConfiguration.Add(new SensorConfiguration("Near", sonar.Contract.DeviceModel, string.Empty, 0.0, 20.0));
_state.SensorConfiguration.Add(new SensorConfiguration("Far", sonar.Contract.DeviceModel, string.Empty, 220.0, 255.0));
}
// Clear Runtime Configuration
_state.RuntimeConfiguration = null;
UnsubscribeAllSensors();
// Clear generic state
if (_contactSensorArrayState == null)
{
_contactSensorArrayState = new bumper.ContactSensorArrayState();
}
if (_contactSensorArrayState.Sensors == null)
{
_contactSensorArrayState.Sensors = new List <bumper.ContactSensor>();
}
if (saveState)
{
SaveState(_state);
}
// Initialize Runtime Configuration
_state.RuntimeConfiguration = new Dictionary <SensorRange, PortConfiguration>();
}
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 virtual IEnumerator<ITask> RemoveDeviceConfigurationHandler(ResetConfiguration resetConfiguration)
{
// Clear Runtime Configuration
UnsubscribeAllSensors();
_state.SensorConfiguration = new List<SensorConfiguration>();
_state.RuntimeConfiguration = null;
_contactSensorArrayState = new bumper.ContactSensorArrayState();
resetConfiguration.ResponsePort.Post(DefaultDeleteResponseType.Instance);
yield break;
}
public virtual IEnumerator<ITask> ReplaceHandler(bumper.Replace replace)
{
_state = replace.Body;
replace.ResponsePort.Post(DefaultReplaceResponseType.Instance);
yield break;
}
public BumpersArrayUpdate(bumper.ContactSensorArrayState 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();
}
public virtual IEnumerator <ITask> ReplaceHandler(bumper.Replace replace)
{
_state = replace.Body;
replace.ResponsePort.Post(DefaultReplaceResponseType.Instance);
yield break;
}
public IEnumerator<ITask> ReplaceHandler(pxContactSensor.Replace replace)
{
_state = replace.Body;
replace.ResponsePort.Post(dssp.DefaultReplaceResponseType.Instance);
_subMgrPort.Post(new submgr.Submit(_state, dssp.DsspActions.ReplaceRequest));
yield break;
}
/// <summary>
/// Initialize State
/// </summary>
private void InitializeState()
{
if (_state == null)
_state = new TouchSensorState();
if (_state.PollingFrequencyMs == 0)
_state.PollingFrequencyMs = Contract.DefaultPollingFrequencyMs;
_state.TimeStamp = DateTime.MinValue;
_state.Connected = false;
_state.TouchSensorOn = false;
if (_genericContactState == null)
{
_genericContactState = new contactsensor.ContactSensorArrayState();
}
if (_genericContactState.Sensors == null)
{
_genericContactState.Sensors = new List<contactsensor.ContactSensor>();
}
if (_genericContactState.Sensors.Count == 0)
{
_genericContactState.Sensors.Add(new contactsensor.ContactSensor(0, "Touch Sensor"));
}
}
/// <summary>
/// Validate the State.
/// </summary>
private void ValidateState()
{
bool saveState = false;
if (_state == null)
{
saveState = true;
_state = new NxtContactSensorArrayState();
}
if (_state.SensorConfiguration == null)
{
saveState = true;
_state.SensorConfiguration = new List<SensorConfiguration>();
}
if (_state.SensorConfiguration.Count == 0)
{
saveState = true;
_state.SensorConfiguration.Add(new SensorConfiguration("Pressed", touch.Contract.DeviceModel, string.Empty, 1.0, 1.0));
_state.SensorConfiguration.Add(new SensorConfiguration("Near", sonar.Contract.DeviceModel, string.Empty, 0.0, 20.0));
_state.SensorConfiguration.Add(new SensorConfiguration("Far", sonar.Contract.DeviceModel, string.Empty, 220.0, 255.0));
}
// Clear Runtime Configuration
_state.RuntimeConfiguration = null;
UnsubscribeAllSensors();
// Clear generic state
if (_contactSensorArrayState == null)
_contactSensorArrayState = new bumper.ContactSensorArrayState();
if (_contactSensorArrayState.Sensors == null)
_contactSensorArrayState.Sensors = new List<bumper.ContactSensor>();
if (saveState)
SaveState(_state);
// Initialize Runtime Configuration
_state.RuntimeConfiguration = new Dictionary<SensorRange, PortConfiguration>();
}
private bool isSensingRearObstruction(bool leftWhiskerPressed, bool rightWhiskerPressed, bumper.ContactSensorArrayState bumpersState)
{
return(!leftWhiskerPressed && !rightWhiskerPressed);
}
/// <summary>
/// Stops the robot. If the robot was going forward it backs up.
/// Keep in mind that a "hardware" stop has already been initiated in the Power Brick, so by the time we get here wheels are stopped.
/// </summary>
protected void Bumped(bool leftWhiskerPressed, bool rightWhiskerPressed, bumper.ContactSensorArrayState bumpersState)
{
LogInfo("TrackRoamerBehaviorsService: Bumped() _state.Velocity=" + _state.Velocity + " _state.MovingState=" + _state.MovingState);
string whatIsBumped;
int turnFactor; // = btRand.NextDouble() > 0.5d ? 1 : -1;
if (leftWhiskerPressed && rightWhiskerPressed)
{
whatIsBumped = "Both whiskers";
turnFactor = 0; // straight back
}
if (leftWhiskerPressed)
{
whatIsBumped = "Left whisker";
turnFactor = -1; // turning a bit to the right, to avoid obstacle to the left
}
else if (rightWhiskerPressed)
{
whatIsBumped = "Right whisker";
turnFactor = 1; // turning a bit to the left, to avoid obstacle to the right
}
else
{
whatIsBumped = "Proximity array";
turnFactor = btRand.NextDouble() > 0.5d ? 1 : -1;
}
Talker.Say(3, "bumped " + whatIsBumped);
if (!_testBumpMode && _state.Velocity < 0)
{
// we are moving backwards,
// front whiskers ignored when we move backwards. Others cause immediate stop:
if (isSensingRearObstruction(leftWhiskerPressed, rightWhiskerPressed, bumpersState))
{
LogInfo("TrackRoamerBehaviorsService: Bumped() - only Rear proximity sensors pressed while moving backwards, stopping...");
// either a rear bumper or both front and rear
// bumpers are pressed. STOP!
StopTurning();
//StopMoving();
// whatever it was, we didn't expect it. Let higher level decision-making take over, may be look around, do mapping:
_state.MovingState = MovingState.Unknown;
_state.Countdown = 3;
}
else
{
LogInfo("TrackRoamerBehaviorsService: Bumped() while moving backwards, whisker press ignored (robot stopped anyway)");
Talker.Say(4, whatIsBumped + " ignored");
// well, motors are stopped and we are not getting completion by encoders.
_state.MovingState = MovingState.Unknown;
_state.Countdown = 3;
}
}
else
{
// we are moving forward, or in test mode.
// _testBumpMode always ends here - even if we are stationary or moving backwards
if (_state.MovingState != MovingState.BumpedBackingUp)
{
_state.MovingState = MovingState.BumpedBackingUp;
lastBumpedBackingUpStarted = DateTime.Now;
int angle = BackupAngleDegrees * turnFactor;
Tracer.Trace("TrackRoamerBehaviorsService: Bumped() - " + whatIsBumped + " pressed, backing up by " + (-BackupDistanceMm) + " mm turning " + angle + " degrees");
Talker.Say(4, "backing up");
// only a front bumper is pressed.
// move back <BackupDistance> mm;
TurnAndMoveParameters tamp = new TurnAndMoveParameters()
{
distance = BackupDistanceMm,
speed = (int)Math.Round(ModerateBackwardVelocityMmSec),
rotatePower = ModerateTurnPower,
rotateAngle = angle
};
Port <bool> completionPort = BackUpTurnWait(tamp);
// start movement
Activate(Arbiter.Receive(false, completionPort,
delegate(bool b)
{
LogInfo("TrackRoamerBehaviorsService: Bumped() delegate - ++++++++++++ BackUpTurn done ++++++++++++++++++++++++++++++++++++++++++++++++++++");
Talker.Say(4, "done backing up");
// done backing up; let the decision making process take over:
_state.MovingState = MovingState.Unknown;
_state.Countdown = 3;
}
)
);
// exiting here with MovingState.BumpedBackingUp, while the delegate waits for completion.
}
else
{
Talker.Say(4, "whisker press ignored");
// well, motors are stopped and we are not getting completion by encoders.
_state.MovingState = MovingState.Unknown;
_state.Countdown = 3;
}
}
LogInfo("TrackRoamerBehaviorsService: Bumped() exiting - _state.MovingState=" + _state.MovingState);
}
