public virtual IEnumerator <ITask> SendCommandHandler(SendCommand sendCommand)
{
LegoCommand cmd = sendCommand.Body.LegoCommand;
if (buffer == null || _commState.SerialPort == null || !_commState.SerialPort.IsOpen)
{
sendCommand.ResponsePort.Post(Fault.FromException(new InvalidOperationException("Not Connected")));
yield break;
}
if (sendCommand.Body == null || sendCommand.Body.LegoCommand == null)
{
sendCommand.ResponsePort.Post(Fault.FromException(new InvalidOperationException("Invalid LEGO Command")));
yield break;
}
// Dispatch again to one of three different ports:
// If no response is required, send the command immediately
// Otherwise queue into a low or high priority queue to
// be handled one command/response at a time.
if (!sendCommand.Body.LegoCommand.RequireResponse)
{
_commSendImmediatePort.Post(sendCommand);
}
else if (sendCommand.Body.PriorityRequest)
{
_legoPriorityRequestResponsePort.Post(sendCommand);
}
else
{
_legoRequestResponsePort.Post(sendCommand);
}
yield break;
}
public IEnumerator <ITask> DriveDistanceHandler(diffdrive.DriveDistance driveDistance)
{
if (_entity == null)
{
throw new InvalidOperationException("Simulation entity not registered with service");
}
if (!_state.IsEnabled)
{
driveDistance.ResponsePort.Post(Fault.FromException(new Exception("Drive is not enabled.")));
LogError("DriveDistance request to disabled drive.");
yield break;
}
if ((driveDistance.Body.Power > 1.0f) || (driveDistance.Body.Power < -1.0f))
{
// invalid drive power
driveDistance.ResponsePort.Post(Fault.FromException(new Exception("Invalid Power parameter.")));
LogError("Invalid Power parameter in DriveDistanceHandler.");
yield break;
}
_state.DriveDistanceStage = driveDistance.Body.DriveDistanceStage;
if (driveDistance.Body.DriveDistanceStage == diffdrive.DriveStage.InitialRequest)
{
Port <simengine.OperationResult> entityResponse = new Port <simengine.OperationResult>();
Activate(Arbiter.Receive <simengine.OperationResult>(false, entityResponse, delegate(simengine.OperationResult result)
{
// post a message to ourselves indicating that the drive distance has completed
diffdrive.DriveDistanceRequest req = new diffdrive.DriveDistanceRequest(0, 0);
switch (result)
{
case simengine.OperationResult.Error:
req.DriveDistanceStage = diffdrive.DriveStage.Canceled;
break;
case simengine.OperationResult.Canceled:
req.DriveDistanceStage = diffdrive.DriveStage.Canceled;
break;
case simengine.OperationResult.Completed:
req.DriveDistanceStage = diffdrive.DriveStage.Completed;
break;
}
_mainPort.Post(new diffdrive.DriveDistance(req));
}));
_entity.DriveDistance((float)driveDistance.Body.Distance, (float)driveDistance.Body.Power, entityResponse);
diffdrive.DriveDistanceRequest req2 = new diffdrive.DriveDistanceRequest(0, 0);
req2.DriveDistanceStage = diffdrive.DriveStage.Started;
_mainPort.Post(new diffdrive.DriveDistance(req2));
}
else
{
base.SendNotification(_subMgrPort, driveDistance);
}
driveDistance.ResponsePort.Post(DefaultUpdateResponseType.Instance);
yield break;
}
public virtual IEnumerator <ITask> ReceiveBluetoothMessageHandler(ReceiveBluetoothMessage query)
{
BluetoothMessage response = new BluetoothMessage();
int inbox = Math.Max(1, Math.Min(10, query.Body.Mailbox)) - 1;
nxtcmd.LegoMessageRead cmd = new nxtcmd.LegoMessageRead(inbox + 10, inbox, true);
yield return(Arbiter.Choice(_legoBrickPort.SendNxtCommand(cmd),
delegate(nxtcmd.LegoResponse ok)
{
nxtcmd.LegoResponseMessageRead queryResponse = nxtcmd.LegoResponse.Upcast <nxtcmd.LegoResponseMessageRead>(ok);
if (queryResponse.Success)
{
response.Mailbox = query.Body.Mailbox;
response.Message = queryResponse.Message;
query.ResponsePort.Post(response);
}
else
{
query.ResponsePort.Post(Fault.FromException(new System.IO.IOException(queryResponse.ErrorCode.ToString())));
}
},
delegate(Fault fault)
{
query.ResponsePort.Post(fault);
}));
yield break;
}
public virtual IEnumerator <ITask> DeleteNamedScriptHandler(DeleteNamedScript deleteNamedScript)
{
if (string.IsNullOrEmpty(deleteNamedScript.Body.Name))
{
deleteNamedScript.ResponsePort.Post(Fault.FromException(new InvalidOperationException("Delete Failed: Script was not specified.")));
yield break;
}
if (deleteNamedScript.Body.Name.Equals("all", StringComparison.InvariantCultureIgnoreCase))
{
_state.SavedScripts = new List <ScriptDefinition>();
}
else
{
int ix = _state.SavedScripts.FindIndex(delegate(ScriptDefinition script) { return(deleteNamedScript.Body.Name == script.Name); });
if (ix >= 0)
{
_state.SavedScripts.RemoveAt(ix);
}
}
// Save the state
SaveState(_state);
// Success
deleteNamedScript.ResponsePort.Post(DefaultDeleteResponseType.Instance);
}
public IEnumerator <ITask> SetSpeedHandler(diffdrive.SetDriveSpeed setSpeed)
{
if (_entity == null)
{
throw new InvalidOperationException("Simulation entity not registered with service");
}
if (!_state.IsEnabled)
{
setSpeed.ResponsePort.Post(Fault.FromException(new Exception("Drive is not enabled.")));
LogError("SetSpeed request to disabled drive.");
yield break;
}
_entity.SetVelocity(
(float)setSpeed.Body.LeftWheelSpeed,
(float)setSpeed.Body.RightWheelSpeed);
UpdateStateFromSimulation();
setSpeed.ResponsePort.Post(DefaultUpdateResponseType.Instance);
// send update notification for entire state
_subMgrPort.Post(new submgr.Submit(_state, DsspActions.UpdateRequest));
yield break;
}
public virtual IEnumerator<ITask> WriteTextHandler(stream.WriteText submit)
{
if (submit == null || submit.Body == null || submit.Body.Text == null)
{
submit.ResponsePort.Post(Fault.FromException(new InvalidOperationException("Invalid data sent to the iRobot.")));
yield break;
}
if (_serialPort == null || !_serialPort.IsOpen)
{
submit.ResponsePort.Post(Fault.FromException(new InvalidOperationException("Attempting to send a command before the iRobot Connection has been established.")));
yield break;
}
try
{
if (submit.Body.Text.EndsWith("\r") || submit.Body.Text.EndsWith("\n"))
_serialPort.Write(submit.Body.Text);
else
_serialPort.WriteLine(submit.Body.Text);
}
catch (Exception ex)
{
LogError(LogGroups.Console, ex);
submit.ResponsePort.Post(Fault.FromException(ex));
yield break;
}
submit.ResponsePort.Post(DefaultSubmitResponseType.Instance);
yield break;
}
public virtual IEnumerator <ITask> QueryBrickNameHandler(QueryBrickName query)
{
BrickNameResponse response = new BrickNameResponse();
nxtcmd.LegoGetDeviceInfo cmd = new nxtcmd.LegoGetDeviceInfo();
yield return(Arbiter.Choice(_legoBrickPort.SendNxtCommand(cmd),
delegate(nxtcmd.LegoResponse ok)
{
nxtcmd.LegoResponseGetDeviceInfo queryResponse = nxtcmd.LegoResponse.Upcast <nxtcmd.LegoResponseGetDeviceInfo>(ok);
if (queryResponse.Success)
{
response.BrickName = queryResponse.BrickName;
query.ResponsePort.Post(response);
}
else
{
query.ResponsePort.Post(Fault.FromException(new System.IO.FileNotFoundException(queryResponse.ErrorCode.ToString())));
}
},
delegate(Fault fault)
{
query.ResponsePort.Post(fault);
}));
yield break;
}
public virtual IEnumerator<ITask> ClearStreamBuffersHandler(stream.ClearStreamBuffers clearBuffers)
{
if (_serialPort != null && _serialPort.IsOpen)
{
try
{
if (clearBuffers.Body.ClearReadBuffer)
{
if (_serialPort.BytesToRead > 0)
_serialPort.DiscardInBuffer();
}
if (clearBuffers.Body.ClearWriteBuffer)
{
if (_serialPort.BytesToWrite > 0)
_serialPort.DiscardOutBuffer();
}
clearBuffers.ResponsePort.Post(DefaultSubmitResponseType.Instance);
yield break;
}
catch (Exception ex)
{
clearBuffers.ResponsePort.Post(Fault.FromException(ex));
yield break;
}
}
clearBuffers.ResponsePort.Post(Fault.FromException(new InvalidOperationException("iRobot Connection is not initialized")));
yield break;
}
public virtual IEnumerator <ITask> QueryRunningLegoProgramHandler(QueryRunningProgram query)
{
RunningProgramResponse response = new RunningProgramResponse();
nxtcmd.LegoGetCurrentProgramName cmd = new nxtcmd.LegoGetCurrentProgramName();
yield return(Arbiter.Choice(_legoBrickPort.SendNxtCommand(cmd),
delegate(nxtcmd.LegoResponse ok)
{
nxtcmd.LegoResponseGetCurrentProgramName queryResponse = nxtcmd.LegoResponse.Upcast <nxtcmd.LegoResponseGetCurrentProgramName>(ok);
if (queryResponse.Success)
{
response.Program = queryResponse.FileName;
query.ResponsePort.Post(response);
}
else
{
query.ResponsePort.Post(Fault.FromException(new System.IO.FileNotFoundException(queryResponse.ErrorCode.ToString())));
}
},
delegate(Fault fault)
{
query.ResponsePort.Post(fault);
}));
yield break;
}
public void DriveSetPowerHandler(diffdrive.SetDrivePower setPower)
{
if (this.driveEntity == null)
{
throw new InvalidOperationException("Simulation entity not registered with service");
}
if (!this.state.DriveState.IsEnabled)
{
setPower.ResponsePort.Post(Fault.FromException(new Exception("Drive is not enabled.")));
LogError("SetPower request to disabled drive.");
return;
}
if ((setPower.Body.LeftWheelPower > 1.0f) || (setPower.Body.LeftWheelPower < -1.0f) ||
(setPower.Body.RightWheelPower > 1.0f) || (setPower.Body.RightWheelPower < -1.0f))
{
// invalid drive power
setPower.ResponsePort.Post(Fault.FromException(new Exception("Invalid Power parameter.")));
LogError("Invalid Power parameter in SetPowerHandler.");
return;
}
// Call simulation entity method for setting wheel torque
this.driveEntity.SetMotorTorque((float)setPower.Body.LeftWheelPower, (float)setPower.Body.RightWheelPower);
this.UpdateStateFromSimulation();
setPower.ResponsePort.Post(DefaultUpdateResponseType.Instance);
// send update notification for entire state
this.subMgrPort.Post(new submgr.Submit(this.state.DriveState, DsspActions.UpdateRequest));
}
public virtual IEnumerator <ITask> SpotlightHandler(Spotlight spotlight)
{
_state.SpotlightOn = spotlight.Body.SpotlightOn;
LegoSetInputMode cmd = new LegoSetInputMode(_state.SensorPort, _state.SpotlightOn ? LegoSensorType.LightActive : LegoSensorType.LightInactive, LegoSensorMode.PercentFullScaleMode);
_legoBrickPort.SendNxtCommand(cmd);
yield return(Arbiter.Choice(_legoBrickPort.SendNxtCommand(cmd),
delegate(LegoResponse response)
{
if (response.Success)
{
spotlight.ResponsePort.Post(DefaultUpdateResponseType.Instance);
// Spotlight notifications are only sent to subscribers to the native service
SendNotification <Spotlight>(_subMgrPort, spotlight);
}
else
{
spotlight.ResponsePort.Post(
Fault.FromException(
new InvalidOperationException(response.ErrorCode.ToString())));
}
},
delegate(Fault fault)
{
spotlight.ResponsePort.Post(fault);
}));
yield break;
}
/// <summary>
/// Initialize the Depth Camera
/// </summary>
/// <returns>An iterator</returns>
private IEnumerator <ITask> ConnectDepthCamHandler()
{
Fault fault = null;
yield return(Arbiter.Choice(
this.depthCamSensorPort.Subscribe(this.depthCamSensorNotify),
EmptyHandler,
f => fault = f));
if (fault != null)
{
LogError(null, "Failed to subscribe to DepthCam", fault);
yield break;
}
var runForm = new RunForm(this.CreateDepthCamForm);
WinFormsServicePort.Post(runForm);
yield return(Arbiter.Choice(runForm.pResult, EmptyHandler, e => fault = Fault.FromException(e)));
if (fault != null)
{
LogError(null, "Failed to Create DepthCam window", fault);
yield break;
}
yield break;
}
public void DriveRotateHandler(diffdrive.RotateDegrees rotate)
{
if (this.driveEntity == null)
{
throw new InvalidOperationException("Simulation entity not registered with service");
}
if (!this.state.DriveState.IsEnabled)
{
rotate.ResponsePort.Post(Fault.FromException(new Exception("Drive is not enabled.")));
LogError("RotateDegrees request to disabled drive.");
return;
}
this.state.DriveState.RotateDegreesStage = rotate.Body.RotateDegreesStage;
if (rotate.Body.RotateDegreesStage == diffdrive.DriveStage.InitialRequest)
{
var entityResponse = new Port <engine.OperationResult>();
Activate(
Arbiter.Receive(
false,
entityResponse,
result =>
{
// post a message to ourselves indicating that the drive distance has completed
var req = new diffdrive.RotateDegreesRequest(0, 0);
switch (result)
{
case engine.OperationResult.Error:
req.RotateDegreesStage = diffdrive.DriveStage.Canceled;
break;
case engine.OperationResult.Canceled:
req.RotateDegreesStage = diffdrive.DriveStage.Canceled;
break;
case engine.OperationResult.Completed:
req.RotateDegreesStage = diffdrive.DriveStage.Completed;
break;
}
this.drivePort.Post(new diffdrive.RotateDegrees(req));
}));
this.driveEntity.RotateDegrees((float)rotate.Body.Degrees, (float)rotate.Body.Power, entityResponse);
var req2 = new diffdrive.RotateDegreesRequest(0, 0)
{
RotateDegreesStage = diffdrive.DriveStage.Started
};
this.drivePort.Post(new diffdrive.RotateDegrees(req2));
}
else
{
SendNotification(this.subMgrPort, rotate);
}
rotate.ResponsePort.Post(DefaultUpdateResponseType.Instance);
}
/// <summary>
/// UploadSrgsFile HTTP post handler
/// </summary>
/// <param name="postData"></param>
/// <param name="post"></param>
/// <returns></returns>
private IEnumerator <ITask> UploadSrgsFileHttpPostHandler(HttpPostRequestData postData, HttpPost post)
{
string mountPointFileLocation = postData.Parameters["MountPointSaveLocation"];
if (string.IsNullOrEmpty(mountPointFileLocation))
{
PostHttpPostParameterError(post, "No mount point save location was specified");
yield break;
}
HttpPostFile file = postData.Files["SrgsFile"];
if (file == null)
{
PostHttpPostParameterError(post, "No SRGS file uploaded");
yield break;
}
// Write file to mount service
SuccessFailurePort writerResponsePort = new SuccessFailurePort();
yield return(new IterativeTask(delegate
{
return WriteFileToMountService(
mountPointFileLocation,
file.InputStream,
writerResponsePort
);
}));
Exception writeException = (Exception)writerResponsePort;
if (writeException != null)
{
LogWarning(writeException);
post.ResponsePort.Post(new HttpResponseType(
HttpStatusCode.BadRequest,
Fault.FromException(writeException)
));
yield break;
}
// Use newly uploaded file
NameValueCollection parameters = new NameValueCollection();
parameters["SrgsFileLocation"] = mountPointFileLocation;
yield return(new IterativeTask(delegate
{
return UseExistingSrgsFileHttpPostHandler(
parameters,
post
);
}));
// UseExistingSrgsFileHttpPostHandler already took care of posting
// a result on the response port
yield break;
}
/// <summary>
/// SaveDictionary HTTP post handler
/// </summary>
/// <param name="parameters">Dictionary entries</param>
/// <param name="post">Request from the web page</param>
/// <returns></returns>
private IEnumerator <ITask> SaveDictionaryHttpPostHandler(NameValueCollection parameters, HttpPost post)
{
string[] entryTexts = parameters.GetValues("DictEntryText[]");
string[] entrySemanticValues = parameters.GetValues("DictEntrySemanticValue[]");
if (entryTexts == null || entrySemanticValues == null ||
entryTexts.Length != entrySemanticValues.Length)
{
PostHttpPostParameterError(post, "Both dictionary entry texts and semantic values "
+ "must be posted, and occur in the same quantities");
}
// Set up replace request with new grammar dictionary for SpeechRecognizer service
sr.SpeechRecognizerState srState = new sr.SpeechRecognizerState();
srState.GrammarType = sr.GrammarType.DictionaryStyle;
srState.DictionaryGrammar = new DssDictionary <string, string>();
try
{
for (int i = 0; i < entryTexts.Length; i++)
{
srState.DictionaryGrammar.Add(entryTexts[i].Trim(), entrySemanticValues[i].Trim());
}
}
catch (Exception ex)
{
post.ResponsePort.Post(new HttpResponseType(
HttpStatusCode.OK,
Fault.FromException(ex)
));
yield break;
}
// Post replace request to SpeechRecognizer service and check outcome
sr.Replace replaceRequest = new sr.Replace(srState);
_srPort.Post(replaceRequest);
yield return((Choice)replaceRequest.ResponsePort);
Fault fault = (Fault)replaceRequest.ResponsePort;
if (fault != null)
{
post.ResponsePort.Post(new HttpResponseType(
HttpStatusCode.OK,
fault
));
yield break;
}
_state.SpeechRecognizerState = srState;
post.ResponsePort.Post(new HttpResponseType(
HttpStatusCode.OK,
HttpPostSuccess.Instance
));
yield break;
}
public IEnumerator <ITask> OnUpdateButton(UpdateButton update)
{
int index = _state.Buttons.FindIndex(update.Body.CompareId);
if (index < 0)
{
update.ResponsePort.Post(
Fault.FromCodeSubcodeReason(
FaultCodes.Receiver,
DsspFaultCodes.UnknownEntry,
"A button with the requested ID does not exist: " + update.Body.Id
)
);
}
else
{
bool isInternal = update.GetHeader <InternalUpdate>() != null;
if (!isInternal)
{
Fault fault = null;
FormInvoke invoke = new FormInvoke(
delegate
{
_form.UpdateButton(update.Body);
}
);
WinFormsServicePort.Post(invoke);
yield return(Arbiter.Choice(
invoke.ResultPort,
EmptyHandler,
delegate(Exception e)
{
fault = Fault.FromException(e);
}
));
if (fault != null)
{
update.ResponsePort.Post(fault);
yield break;
}
}
_state.Buttons[index] = update.Body;
update.ResponsePort.Post(DefaultUpdateResponseType.Instance);
DoSendNotification(update);
}
}
public virtual IEnumerator <ITask> ConfirmHandler(Confirm confirm)
{
PortSet <bool, Exception> result = new PortSet <bool, Exception>();
ConfirmForm form = null;
RunForm runForm = new RunForm(
delegate()
{
form = new ConfirmForm(result);
form.Message = confirm.Body.Message;
form.Countdown = _defaultTimeout;
return(form);
}
);
WinFormsServicePort.Post(runForm);
yield return(Arbiter.Choice(
runForm.pResult,
delegate(SuccessResult success) { },
delegate(Exception e)
{
result.Post(e);
}
));
yield return(Arbiter.Choice(
result,
delegate(bool confirmed)
{
ConfirmResponse response = new ConfirmResponse();
response.Confirmed = confirmed;
confirm.ResponsePort.Post(response);
if (form.Timeout)
{
LogWarning("Confirm dialog cancelled due to timeout.");
}
},
delegate(Exception e)
{
Fault fault = Fault.FromException(e);
LogError(null, "Error in Confirm Handler", fault);
confirm.ResponsePort.Post(fault);
}
));
}
/// <summary>
/// Prompt for New log folder
/// </summary>
/// <param name="resultPort">The result port to post result</param>
internal void PromptForNewLogFolder(PortSet <string, Fault> resultPort)
{
if (MessageBox.Show(
Properties.Resources.XmlToBinaryConversionWarning,
Properties.Resources.XmlToBinaryConversionWarningCaption,
MessageBoxButton.YesNo,
MessageBoxImage.Warning) == MessageBoxResult.Yes)
{
resultPort.Post(this.PresentLoadFileDialog(Properties.Resources.SelectFolderForConvertedBinaryFiles));
}
else
{
resultPort.Post(Fault.FromException(new Exception(Properties.Resources.UserCanceledLoad)));
}
}
public virtual IEnumerator <ITask> AlertHandler(Alert alert)
{
SuccessFailurePort result = new SuccessFailurePort();
AlertForm form = null;
RunForm runForm = new RunForm(
delegate()
{
form = new AlertForm(result);
form.Message = alert.Body.Message;
form.Countdown = _defaultTimeout;
return(form);
}
);
WinFormsServicePort.Post(runForm);
yield return(Arbiter.Choice(
runForm.pResult,
delegate(SuccessResult success){},
delegate(Exception e)
{
result.Post(e);
}
));
yield return(Arbiter.Choice(
result,
delegate(SuccessResult success)
{
alert.ResponsePort.Post(DefaultSubmitResponseType.Instance);
if (form.Timeout)
{
LogWarning("Alert dialog timed out.");
}
},
delegate(Exception e)
{
Fault fault = Fault.FromException(e);
LogError(null, "Error in Alert Handler", fault);
alert.ResponsePort.Post(fault);
}
));
}
public IEnumerator <ITask> OnDeleteButton(DeleteButton delete)
{
FlexButton existing = _state.Buttons.Find(delete.Body.CompareId);
if (existing == null)
{
delete.ResponsePort.Post(
Fault.FromCodeSubcodeReason(
FaultCodes.Receiver,
DsspFaultCodes.UnknownEntry,
"A button with the requested ID does not exist: " + delete.Body.Id
)
);
}
else
{
Fault fault = null;
FormInvoke invoke = new FormInvoke(
delegate
{
_form.DeleteButton(delete.Body);
}
);
WinFormsServicePort.Post(invoke);
yield return(Arbiter.Choice(
invoke.ResultPort,
EmptyHandler,
delegate(Exception e)
{
fault = Fault.FromException(e);
}
));
if (fault != null)
{
delete.ResponsePort.Post(fault);
}
else
{
_state.Buttons.Remove(existing);
delete.ResponsePort.Post(DefaultDeleteResponseType.Instance);
DoSendNotification(delete);
}
}
}
public IEnumerator <ITask> OnInsertButton(InsertButton insert)
{
if (_state.Buttons.Exists(insert.Body.CompareId) ||
_state.Controls.Exists(insert.Body.CompareId))
{
insert.ResponsePort.Post(
Fault.FromCodeSubcodeReason(
FaultCodes.Receiver,
DsspFaultCodes.DuplicateEntry,
"A button with the same ID already exists: " + insert.Body.Id
)
);
}
else
{
Fault fault = null;
FormInvoke invoke = new FormInvoke(
delegate
{
_form.InsertButton(insert.Body);
}
);
WinFormsServicePort.Post(invoke);
yield return(Arbiter.Choice(
invoke.ResultPort,
EmptyHandler,
delegate(Exception e)
{
fault = Fault.FromException(e);
}
));
if (fault != null)
{
insert.ResponsePort.Post(fault);
}
else
{
_state.Buttons.Add(insert.Body);
insert.ResponsePort.Post(DefaultUpdateResponseType.Instance);
DoSendNotification(insert);
}
}
}
public IEnumerator <ITask> SubscribeHandler(webcam.Subscribe subscribe)
{
SubscribeRequestType request = subscribe.Body;
yield return(Arbiter.Choice(
SubscribeHelper(_subMgrPort, request, subscribe.ResponsePort),
delegate(SuccessResult success)
{
SendNotificationToTarget <webcam.Replace>(request.Subscriber, _subMgrPort, _state);
},
delegate(Exception e)
{
LogError(null, "Failure while processing Subscribe Request", Fault.FromException(e));
}
));
yield break;
}
public virtual IEnumerator <ITask> SaveCurrentScriptHandler(SaveCurrentScript saveCurrentScript)
{
if (!string.IsNullOrEmpty(saveCurrentScript.Body.Name))
{
_state.CurrentScript.Name = saveCurrentScript.Body.Name;
}
if (string.IsNullOrEmpty(_state.CurrentScript.Name))
{
saveCurrentScript.ResponsePort.Post(Fault.FromException(new InvalidOperationException("Save Failed: Script is not named.")));
yield break;
}
int ix = _state.SavedScripts.FindIndex(delegate(ScriptDefinition script) { return(saveCurrentScript.Body.Name == script.Name); });
if (ix >= 0)
{
_state.SavedScripts.RemoveAt(ix);
}
// Every script should query for data at the end
// so that we can detect the end of the script.
if (_state.CurrentScript.ExpectedScriptResponseBytes == 0)
{
AddSensors addSensors = new AddSensors();
addSensors.Body = new irobot.CmdSensors(CreateSensorPacket.AllCreate);
AddScriptCommand(addSensors.Body, addSensors.ResponsePort);
yield return(Arbiter.Choice(addSensors.ResponsePort,
delegate(DefaultInsertResponseType ok) { },
delegate(Fault fault) { }));
}
_state.SavedScripts.Add(_state.CurrentScript);
// Clear the current script, ready for a new one.
ClearCurrentScript();
// Save the state
SaveState(_state);
// Success
saveCurrentScript.ResponsePort.Post(_state.CurrentScript);
}
/// <summary>
/// Display an image in the WebCam Form
/// </summary>
/// <param name="bmp">
/// The bitmap to display
/// </param>
private void DisplayImage(Bitmap bmp)
{
Fault fault = null;
var setImage = new FormInvoke(() => this.cameraForm.CameraImage = bmp);
WinFormsServicePort.Post(setImage);
Arbiter.Choice(setImage.ResultPort, EmptyHandler, e => fault = Fault.FromException(e));
if (fault == null)
{
// LogInfo("New camera frame");
return;
}
this.LogError(null, "Unable to set camera image on form", fault);
return;
}
public virtual IEnumerator<ITask> WriteDataHandler(stream.WriteData submit)
{
if (submit == null || submit.Body == null || submit.Body.Data == null)
{
submit.ResponsePort.Post(Fault.FromException(new InvalidOperationException("Invalid data sent to the iRobot.")));
yield break;
}
if (_serialPort == null || !_serialPort.IsOpen)
{
submit.ResponsePort.Post(Fault.FromException(new InvalidOperationException("Attempting to send a command before the iRobot Connection has been established.")));
yield break;
}
if (_iRobotConnection.ConnectionType == roomba.iRobotConnectionType.RooTooth
&& !_serialPort.CtsHolding
&& !_serialPort.DsrHolding)
{
string errorMessage = "The Bluetooth serial port is paired to a device,\r\n"
+ " but the Rootooth may not be connected.\r\n";
LogWarning(LogGroups.Console, errorMessage);
if (!_state.Initialized)
{
submit.ResponsePort.Post(Fault.FromException(new System.IO.IOException(errorMessage)));
yield break;
}
}
try
{
_serialPort.Write(submit.Body.Data, 0, submit.Body.Data.Length);
}
catch (Exception ex)
{
_state.Initialized = false;
LogError(LogGroups.Console, ex);
submit.ResponsePort.Post(Fault.FromException(ex));
yield break;
}
submit.ResponsePort.Post(DefaultSubmitResponseType.Instance);
yield break;
}
public virtual IEnumerator <ITask> CreateDriveDirectHandler(create.CreateDriveDirect update)
{
if (_entity == null)
{
if (update.Body.LeftVelocity == 0 && update.Body.RightVelocity == 0)
{
update.ResponsePort.Post(new irobottypes.RoombaCommandReceived(irobottypes.RoombaMode.NotSpecified));
}
else
{
update.ResponsePort.Post(Fault.FromException(new InvalidOperationException("The Simulation iRobot has not yet been initialized")));
}
yield break;
}
_entity.SetVelocity(update.Body.LeftVelocity / 1000.0f, update.Body.RightVelocity / 1000.0f);
update.ResponsePort.Post(new irobottypes.RoombaCommandReceived(irobottypes.RoombaMode.NotSpecified));
yield break;
}
public virtual IEnumerator <ITask> LoadCurrentScriptHandler(LoadCurrentScript loadCurrentScript)
{
if (string.IsNullOrEmpty(loadCurrentScript.Body.Name))
{
loadCurrentScript.ResponsePort.Post(Fault.FromException(new InvalidOperationException("Load Failed: Script must have a name.")));
yield break;
}
int ix = _state.SavedScripts.FindIndex(delegate(ScriptDefinition script) { return(loadCurrentScript.Body.Name == script.Name); });
if (ix < 0)
{
loadCurrentScript.ResponsePort.Post(Fault.FromException(new InvalidOperationException("Load Failed: Script " + loadCurrentScript.Body.Name + " not found.")));
yield break;
}
_state.CurrentScript = _state.SavedScripts[ix];
// Success
loadCurrentScript.ResponsePort.Post(_state.CurrentScript);
yield break;
}
public virtual IEnumerator <ITask> DeleteFileHandler(DeleteFile deleteFile)
{
nxtcmd.LegoDelete cmd = new nxtcmd.LegoDelete(deleteFile.Body.FileName);
yield return(Arbiter.Choice(_legoBrickPort.SendNxtCommand(cmd),
delegate(nxtcmd.LegoResponse ok)
{
if (ok.Success || ok.ErrorCode == LegoErrorCode.FileNotFound)
{
deleteFile.ResponsePort.Post(DefaultSubmitResponseType.Instance);
}
else
{
deleteFile.ResponsePort.Post(Fault.FromException(new System.IO.IOException(ok.ErrorCode.ToString())));
}
},
delegate(Fault fault)
{
deleteFile.ResponsePort.Post(fault);
}));
yield break;
}
IEnumerator <ITask> DoBlobTrackerImageProcessedHandler(bt.ImageProcessed processed)
{
try
{
Fault fault = null;
FormInvoke setTracking = new FormInvoke(
delegate
{
if (!_form.IsDisposed)
{
_form.Tracking = processed.Body.Results;
}
}
);
WinFormsServicePort.Post(setTracking);
yield return(Arbiter.Choice(
setTracking.ResultPort,
delegate(EmptyValue success) { },
delegate(Exception e)
{
fault = Fault.FromException(e);
}
));
if (fault != null)
{
LogError(null, "Unable to set tracking information", fault);
yield break;
}
}
finally
{
_mainPort.Post(new UpdateProcessing(false));
}
}
/// <summary>
/// Handles the <typeparamref name="LaserRangeFinderUpdate"/> request.
/// </summary>
/// <param name="update">request</param>
void LaserRangeFinderUpdateHandler(LaserRangeFinderUpdate update)
{
try
{
sicklrf.State laserData = update.Body;
_state.MostRecentLaser = laserData.TimeStamp;
int distance = FindNearestObstacleInCorridor(laserData, CorridorWidthMapping, 45);
// AvoidCollision and EnterOpenSpace have precedence over
// all other state transitions and are thus handled first.
AvoidCollision(distance);
EnterOpenSpace(distance);
UpdateLogicalState(laserData, distance);
update.ResponsePort.Post(DefaultUpdateResponseType.Instance);
}
catch (Exception e)
{
update.ResponsePort.Post(Fault.FromException(e));
}
}