protected void BumperHandler(bumper.Update notification)
{
if (string.IsNullOrEmpty(notification.Body.Name))
{
LogInfo(LogGroups.Console, "Bumper name is null or empty.");
return;
}
string bumperName = notification.Body.Name.ToLowerInvariant();
//Trace.WriteLine(bumperName + " @ " + DateTime.Now + ": " + notification.Body.Pressed);
if (bumperName.Contains("front"))
{
lock (_soar.Bumper)
{
_soar.Bumper.FrontBumperPressed = notification.Body.Pressed;
}
}
else if (bumperName.Contains("rear"))
{
lock (_soar.Bumper)
{
_soar.Bumper.RearBumperPressed = notification.Body.Pressed;
}
}
}
/// <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;
}
public virtual IEnumerator<ITask> ReliableSubscribeHandler(contactsensor.ReliableSubscribe subscribe)
{
yield return (Choice)base.SubscribeHelper(_subMgrPort, subscribe.Body, subscribe.ResponsePort);
}
public virtual IEnumerator<ITask> GenericSubscribeHandler(bumper.Subscribe subscribe)
{
base.SubscribeHelper(_subMgrPort, subscribe.Body, subscribe.ResponsePort);
yield break;
}
/// <summary>
/// Update the UI with the estat of a single bumper
/// </summary>
/// <param name="bumperState"></param>
public void UpdateBumperData(pxbumper.ContactSensor bumperState)
{
UpdateBumperButton(bumperState.Name, bumperState.Pressed);
}
/// <summary>
/// Handles Update notification from the Bumper partner
/// </summary>
/// <remarks>Posts a <typeparamref name="BumperUpdate"/> to itself.</remarks>
/// <param name="update">notification</param>
void BumperUpdateNotification(bumper.Update update)
{
LogInfo("Explorer: BumperUpdateNotification()");
_mainPort.Post(new BumperUpdate(update.Body));
}
public virtual IEnumerator<ITask> ReplaceHandler(bumper.Replace replace)
{
_state = replace.Body;
replace.ResponsePort.Post(DefaultReplaceResponseType.Instance);
yield break;
}
/// <summary>
/// Handles bumper update notification
/// </summary>
/// <param name="update">bumper update notification</param>
/// <returns></returns>
IEnumerator<ITask> OnBumperUpdateHandler(pxbumper.Update update)
{
if (_driveControl != null)
{
WinFormsServicePort.FormInvoke(
delegate()
{
_driveControl.UpdateBumperData(update.Body);
}
);
}
yield break;
}
public virtual IEnumerator<ITask> ReliableSubscribeHandler(pxbumper.ReliableSubscribe subscribe)
{
LogInfo("TrackRoamerBumper received Reliable Subscription request from Subscriber=" + subscribe.Body.Subscriber);
base.SubscribeHelper(_subMgrPort, subscribe.Body, subscribe.ResponsePort);
yield break;
}
// Robotics Tutorial 1 Step 5 -- Create a handler for the subscription
// The Robotics Tutorial says to add this at the end
// of the file, but I don't know why because it compiles
// wherever you put it ...
// There seems to be a major problem with the approach in
// Robotics Tutorial 3. It results in the robot jerking up
// against the wall because of a backlog of queued messages.
/// <summary>
/// Handle Bumper Notifications
/// </summary>
/// <param name="notification"></param>
void BumperHandler(bumper.Update notification)
{
string message;
string bumperName;
DateTime thisTimestamp;
TimeSpan timediff;
// Find out which bumper this is
// TT - Nov CTP
// int num = notification.Body.Identifier;
int num = notification.Body.HardwareIdentifier;
// Robotics Tutorial 3:
//contactsensor.ContactSensor s = updateNotification.Body;
if (string.IsNullOrEmpty(notification.Body.Name))
bumperName = "";
else
bumperName = notification.Body.Name.ToLowerInvariant();
if (!notification.Body.Pressed)
{
message = "Bumper " + num.ToString() + " (" + bumperName + ") was released.";
LogInfo(LogGroups.Console, message);
return;
}
else
{
message = "Ouch! Bumper " + num.ToString() + " (" + bumperName + ") was pressed.";
LogInfo(LogGroups.Console, message);
StopNow();
}
// OK, we have a bump!
// What if it is the OPPOSITE bumper?
// This should be handled better.
// In particular, there are instances where both front
// AND back bumpers are triggered! It would be a better
// idea to remember the direction of motion and always
// back up when a bumper notification is received.
// Get the current time
thisTimestamp = System.DateTime.Now;
// Make sure that we have waited long enough for the last
// motion to complete. This is a fudge that effectively
// renders the robot blind for a short period of time.
// Not really a good solution if there is a chance that
// the opposite bumper will be triggered legitimately.
// It is sometimes referred to as a "dead zone" and is
// similar to what is used in simple "bang bang" or
// "on/off" control to avoid the continuous oscillations.
if (thisTimestamp < _state.nextTimestamp)
{
int count = 0;
bumper.Replace testBump = null;
bumper.Replace testBump2 = null;
int skip = bumperNotificationPort.P3.ItemCount;
// We don't want any more bump notifications while we
// are processing the current one.
// This code for skipping notifications uses a pattern
// from the Traxster motor.cs. However, the types of
// messages involved are different so it is not just a
// copy and paste :-(
// Note that the queue can easily get over a thousand
// messages in a short space of time!
for (int i = 0; i < skip; i++)
{
testBump = (bumper.Replace) bumperNotificationPort.P3.Test();
if (testBump != null)
{
count++;
// reply and pretend everything went well
testBump.ResponsePort.Post(DefaultReplaceResponseType.Instance);
}
}
if (count > 0)
{
message = "Ignored " + num.ToString() + " - " + count.ToString() + " messages";
// LogInfo(LogGroups.Console, message);
}
// Return without doing anything
return;
}
// Wait for the motion to complete for this amount
// of time, i.e. ignore future bumps!!!
// This is a fudge because we don't know for sure
// how long any particular motion will take, but
// it's the best we can do.
_state.nextTimestamp = thisTimestamp.AddMilliseconds(3500);
// From Robotics Tutorial 3
// Look carefully at the code. At first glance, it seems
// to call the same function for either bumper, but the
// polarity parameter is different.
if (bumperName.Contains("front"))
{
if (_state.insideBehavior <= 0)
{
LogInfo(LogGroups.Console, ">>> Start -1");
// Set a flag so that multiple threads
// will not execute the behavior at the
// same time. This is possible, but the
// motor commands will get "interleaved"
// and the robot does some really strange
// things!
_state.insideBehavior++;
// Create a new iterator to execute the
// behavior
SpawnIterator<double>(-1, BackUpTurnAndMove);
}
return;
}
if (bumperName.Contains("rear"))
{
if (_state.insideBehavior <= 0)
{
LogInfo(LogGroups.Console, ">>> Start +1");
// Set the synchronization flag
_state.insideBehavior++;
SpawnIterator<double>(1, BackUpTurnAndMove);
}
return;
}
}
/// <summary>
/// Handles Update notification from the Bumper partner
/// </summary>
/// <remarks>Posts a <typeparamref name="BumperUpdate"/> to itself.</remarks>
/// <param name="update">notification</param>
void BumperUpdateNotification(bumper.Update update)
{
_mainPort.Post(new BumperUpdate(update.Body));
}
/// <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);
}
public virtual IEnumerator<ITask> GetContatSensorHandler(contactsensor.Get get)
{
get.ResponsePort.Post(SyncGenericContactState());
yield break;
}
private bool isSensingRearObstruction(bool leftWhiskerPressed, bool rightWhiskerPressed, bumper.ContactSensorArrayState bumpersState)
{
return !leftWhiskerPressed && !rightWhiskerPressed;
}
public IEnumerator<ITask> ReliableSubscribeHandler(pxContactSensor.ReliableSubscribe subscribe)
{
yield return Arbiter.Choice(
SubscribeHelper(_subMgrPort, subscribe.Body, subscribe.ResponsePort),
delegate(SuccessResult success)
{
_subMgrPort.Post(new submgr.Submit(
subscribe.Body.Subscriber, dssp.DsspActions.ReplaceRequest, _state, null));
},
delegate(Exception ex) { LogError(ex); }
);
}
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;
}
public IEnumerator<ITask> GetHandler(pxContactSensor.Get get)
{
get.ResponsePort.Post(_state);
yield break;
}
public virtual IEnumerator<ITask> GenericUpdateHandler(bumper.Update update)
{
throw new InvalidOperationException("Contact Sensor Update is a Notification and not valid in this context.");
}
/// <summary>
/// Handles Replace notifications from the Bumper partner
/// </summary>
/// <remarks>Posts a <typeparamref name="BumpersUpdate"/> to itself.</remarks>
/// <param name="replace">notification</param>
void BumperReplaceNotification(bumper.Replace replace)
{
_mainPort.Post(new BumpersUpdate(replace.Body));
}
public virtual IEnumerator<ITask> ReplaceHandler(contactsensor.Replace replace)
{
replace.ResponsePort.Post(Fault.FromException(new InvalidOperationException("The LEGO NXT touch sensor is updated from hardware.")));
yield break;
}
/// <summary>
/// Handlers Bumpers Replace notification
/// </summary>
/// <param name="replace">Bumpers replace notification</param>
/// <returns></returns>
IEnumerator<ITask> OnBumpersReplaceHandler(pxbumper.Replace replace)
{
if (_driveControl != null)
{
WinFormsServicePort.FormInvoke(
delegate()
{
_driveControl.ReplaceBumpersData(replace.Body);
}
);
}
LogObject(replace.Body);
yield break;
}
public virtual IEnumerator<ITask> SubscribeHandler(contactsensor.Subscribe subscribe)
{
yield return (Choice)base.SubscribeHelper(_subMgrPort, subscribe.Body, subscribe.ResponsePort);
if (_state.TimeStamp != DateTime.MinValue)
{
SendNotificationToTarget<contactsensor.Replace>(subscribe.Body.Subscriber, _genericSubMgrPort, SyncGenericContactState());
}
}
public virtual IEnumerator<ITask> UpdateHandler(pxbumper.Update updateBumper)
{
throw new InvalidOperationException("Track Roamer Bumper Sensors are not updateable");
}
/// <summary>
/// Handles Update notification from the Bumper partner - updates a single sensor state (usually whiskers)
/// </summary>
/// <remarks>Posts a <typeparamref name="BumperUpdate"/> to itself.</remarks>
/// <param name="update">notification</param>
void BumperUpdateNotification(bumper.Update update)
{
LogInfo("DriveBehaviorServiceBase: BumperUpdateNotification()");
_mainPort.Post(new BumperUpdate(update.Body));
}
/// <summary>
/// Handles Replace notifications from the Bumper partner
/// </summary>
/// <remarks>Posts a <typeparamref name="BumpersUpdate"/> to itself.</remarks>
/// <param name="replace">notification</param>
void BumperReplaceNotification(bumper.Replace replace)
{
LogInfo("Explorer: BumperReplaceNotification()");
_mainPort.Post(new BumpersUpdate(replace.Body));
}
/// <summary>
/// Handles Replace notifications from the Bumper partner - updating state of the whole set of sensors
/// </summary>
/// <remarks>Posts a <typeparamref name="BumpersArrayUpdate"/> to itself.</remarks>
/// <param name="replace">notification</param>
void BumperReplaceNotification(bumper.Replace replace)
{
LogInfo("DriveBehaviorServiceBase: BumperReplaceNotification()");
_mainPort.Post(new BumpersArrayUpdate(replace.Body));
}
public virtual IEnumerator<ITask> GetHandler(bumper.Get get)
{
get.ResponsePort.Post(_state);
yield break;
}
// Raul - Bumpers Update code
#region Bumpers Update
/// <summary>
/// Update the UI with the state of bumpers
/// </summary>
/// <param name="bumpersState"></param>
public void ReplaceBumpersData(pxbumper.ContactSensorArrayState bumpersState)
{
foreach (pxbumper.ContactSensor bumper in bumpersState.Sensors)
{
UpdateBumperButton(bumper.Name, bumper.Pressed);
}
}
public IEnumerator<ITask> ReliableSubscribeHandler(bumper.ReliableSubscribe subscribe)
{
yield return Arbiter.Choice(
SubscribeHelper(_subMgrPort, subscribe.Body, subscribe.ResponsePort),
delegate(SuccessResult success)
{
_subMgrPort.Post(new submgr.Submit(
subscribe.Body.Subscriber, DsspActions.ReplaceRequest, _state, null));
},
null
);
}
public virtual IEnumerator<ITask> GenericReplaceHandler(bumper.Replace replace)
{
throw new InvalidOperationException("The LEGO NXT Contact Sensor Array is configured by the LEGO NXT Brick service.");
}
