public IEnumerator <ITask> UpdateHandler(drive.Update update)
{
_state = update.Body;
_state.TimeStamp = DateTime.Now;
update.ResponsePort.Post(new DefaultUpdateResponseType());
yield break;
}
public IEnumerator <ITask> EnableDriveHandler(EnableDrive enableDrive)
{
_state.IsEnabled = enableDrive.Body.Enable;
_state.TimeStamp = DateTime.Now;
// if we are enabling the drive, validate that the motors are configured.
if (enableDrive.Body.Enable)
{
try
{
ValidateDriveConfiguration(false);
}
catch (InvalidOperationException)
{
// If validation fails,
// force the state to not be enabled.
_state.IsEnabled = false;
// rethrow the fault
throw;
}
}
// send notification to subscription manager
drive.Update update = new drive.Update(_state);
SendNotification <drive.Update>(_subMgrPort, update);
enableDrive.ResponsePort.Post(DefaultUpdateResponseType.Instance);
yield break;
}
public void DriveUpdateHandler(drive.Update update)
{
this.controllerDrivePort.Post(
new drive.Update()
{
Body = update.Body, ResponsePort = update.ResponsePort
});
}
public IEnumerator <ITask> ReliableSubscribeHandler(ReliableSubscribe subscribe)
{
yield return(Arbiter.Choice(
SubscribeHelper(_subMgrPort, subscribe.Body, subscribe.ResponsePort),
delegate(SuccessResult success)
{
drive.Update update = new drive.Update(_state);
SendNotificationToTarget <drive.Update>(subscribe.Body.Subscriber, _subMgrPort, update);
},
delegate(Exception ex)
{
LogError(ex);
throw ex;
}
));
}
public IEnumerator <ITask> SetDrivePowerHandler(SetDrivePower setDrivePower)
{
ValidateDriveConfiguration(false);
_state.TimeStamp = DateTime.Now;
PortSet <DefaultUpdateResponseType, Fault> responsePort = new PortSet <DefaultUpdateResponseType, Fault>();
// Add a coordination header to our motor requests
// so that advanced motor implementations can
// coordinate the individual motor reqests.
coord.ActuatorCoordination coordination = new coord.ActuatorCoordination(2);
Motor.SetMotorPower leftPower = new Motor.SetMotorPower(setDrivePower.Body.LeftWheelPower);
leftPower.ResponsePort = responsePort;
leftPower.AddHeader(coordination);
_leftMotorPort.Post(leftPower);
Motor.SetMotorPower rightPower = new Motor.SetMotorPower(setDrivePower.Body.RightWheelPower);
rightPower.ResponsePort = responsePort;
rightPower.AddHeader(coordination);
_rightMotorPort.Post(rightPower);
// send notification to subscription manager
drive.Update update = new drive.Update(_state);
SendNotification <drive.Update>(_subMgrPort, update);
Activate(Arbiter.MultipleItemReceive <DefaultUpdateResponseType, Fault>(responsePort, 2,
delegate(ICollection <DefaultUpdateResponseType> successList, ICollection <Fault> failureList)
{
if (successList.Count == 2)
{
setDrivePower.ResponsePort.Post(new DefaultUpdateResponseType());
}
foreach (Fault fault in failureList)
{
setDrivePower.ResponsePort.Post(fault);
break;
}
}));
yield break;
}
public void DriveUpdateHandler(drive.Update update)
{
throw new NotImplementedException();
}
public IEnumerator<ITask> SubscribeHandler(Subscribe subscribe)
{
yield return Arbiter.Choice(
SubscribeHelper(_subMgrPort, subscribe.Body, subscribe.ResponsePort),
delegate(SuccessResult success)
{
drive.Update update = new drive.Update(_state);
SendNotificationToTarget<drive.Update>(subscribe.Body.Subscriber, _subMgrPort, update);
},
delegate(Exception ex)
{
LogError(ex);
throw ex;
}
);
}
public IEnumerator<ITask> SetDrivePowerHandler(SetDrivePower setDrivePower)
{
ValidateDriveConfiguration(false);
_state.TimeStamp = DateTime.Now;
PortSet<DefaultUpdateResponseType, Fault> responsePort = new PortSet<DefaultUpdateResponseType, Fault>();
// Add a coordination header to our motor requests
// so that advanced motor implementations can
// coordinate the individual motor reqests.
coord.ActuatorCoordination coordination = new coord.ActuatorCoordination(2);
Motor.SetMotorPower leftPower = new Motor.SetMotorPower(setDrivePower.Body.LeftWheelPower);
leftPower.ResponsePort = responsePort;
leftPower.AddHeader(coordination);
_leftMotorPort.Post(leftPower);
Motor.SetMotorPower rightPower = new Motor.SetMotorPower(setDrivePower.Body.RightWheelPower);
rightPower.ResponsePort = responsePort;
rightPower.AddHeader(coordination);
_rightMotorPort.Post(rightPower);
// send notification to subscription manager
drive.Update update = new drive.Update(_state);
SendNotification<drive.Update>(_subMgrPort, update);
Activate(Arbiter.MultipleItemReceive<DefaultUpdateResponseType, Fault>(responsePort, 2,
delegate(ICollection<DefaultUpdateResponseType> successList, ICollection<Fault> failureList)
{
if (successList.Count == 2)
setDrivePower.ResponsePort.Post(new DefaultUpdateResponseType());
foreach (Fault fault in failureList)
{
setDrivePower.ResponsePort.Post(fault);
break;
}
}));
yield break;
}
public IEnumerator<ITask> EnableDriveHandler(EnableDrive enableDrive)
{
_state.IsEnabled = enableDrive.Body.Enable;
_state.TimeStamp = DateTime.Now;
// if we are enabling the drive, validate that the motors are configured.
if (enableDrive.Body.Enable)
{
try
{
ValidateDriveConfiguration(false);
}
catch (InvalidOperationException)
{
// If validation fails,
// force the state to not be enabled.
_state.IsEnabled = false;
// rethrow the fault
throw;
}
}
// send notification to subscription manager
drive.Update update = new drive.Update(_state);
SendNotification<drive.Update>(_subMgrPort, update);
enableDrive.ResponsePort.Post(DefaultUpdateResponseType.Instance);
yield break;
}
/// <summary>
/// Notification handler from underlying drive service
/// </summary>
/// <param name="notifyUpdate">Notification update from drive service</param>
private void DriveNotification(drive.Update notifyUpdate)
{
SendNotification <drive.Update>(this.submgrDrivePort, new drive.Update(notifyUpdate.Body));
}
