public void HandleSetSpeed(gendrive.SetDriveSpeed setSpeed)
{
throw new NotImplementedException();
}
public IEnumerator<ITask> SetSpeedHandler(diffdrive.SetDriveSpeed setSpeed)
{
if (_entity == null)
throw new InvalidOperationException("Simulation entity not registered with service");
_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 IEnumerator<ITask> AllStopHandler(diffdrive.AllStop estop)
{
if (_entity == null)
throw new InvalidOperationException("Simulation entity not registered with service");
_entity.SetMotorTorque(0,0);
_entity.SetVelocity(0);
UpdateStateFromSimulation();
estop.ResponsePort.Post(DefaultUpdateResponseType.Instance);
// send update for entire state
_subMgrPort.Post(new submgr.Submit(_state, DsspActions.UpdateRequest));
yield break;
}
public IEnumerator<ITask> ReliableSubscribeHandler(diffdrive.ReliableSubscribe subscribe)
{
Activate(Arbiter.Choice(
SubscribeHelper(_subMgrPort, subscribe.Body, subscribe.ResponsePort),
delegate(SuccessResult success)
{
_subMgrPort.Post(new submgr.Submit(
subscribe.Body.Subscriber, DsspActions.UpdateRequest, _state, null));
},
delegate(Exception ex) { LogError(ex); }
));
yield break;
}
public IEnumerator<ITask> DriveDistanceHandler(diffdrive.DriveDistance driveDistance)
{
// TT - Added code to support DriveDistance
// Don't waste time if the distance is zero
if (driveDistance.Body.Distance != 0)
_entity.StartTranslate(driveDistance.Body.Distance, driveDistance.Body.Power);
UpdateStateFromSimulation();
driveDistance.ResponsePort.Post(new DefaultUpdateResponseType());
yield break;
}
IEnumerator<ITask> OnDriveUpdateNotificationHandler(drive.Update notification)
{
if (_driveControl != null)
{
WinFormsServicePort.FormInvoke(
delegate()
{
_driveControl.UpdateMotorData(notification.Body);
}
);
}
LogObject(notification.Body);
yield break;
}
protected void NotifyDriveUpdate(drive.Update notification)
{
motorsOn = notification.Body.IsEnabled;
}
public IEnumerator<ITask> DriveDistanceHandler(drive.DriveDistance driveDistance)
{
// If configuration is invalid, an InvalidException is thrown.
ValidateDriveConfiguration(true);
_state.TimeStamp = DateTime.Now;
#if TRACELOG
Tracer.Trace("==================== TrackRoamerDriveService:: DriveDistanceHandler(distance=" + driveDistance.Body.Distance + ")");
#endif
// send immediate response
driveDistance.ResponsePort.Post(DefaultUpdateResponseType.Instance);
// post request to internal port.
_internalDriveOperationsPort.Post(driveDistance);
yield break;
}
public IEnumerator<ITask> EnableDriveHandler(drive.EnableDrive enableDrive)
{
_state.IsEnabled = enableDrive.Body.Enable;
_state.TimeStamp = DateTime.Now;
#if TRACELOG
LogInfo("TrackRoamerDriveService:EnableDriveHandler() - enable=" + _state.IsEnabled);
#endif
// if we are enabling the drive, validate that the motors are configured.
if (enableDrive.Body.Enable)
{
try
{
ValidateDriveConfiguration(true); // we must have encoders for the drive to work properly
}
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 IEnumerator<ITask> AllStopHandler(drive.AllStop allStop)
{
/*
from http://social.msdn.microsoft.com/Forums/en-US/roboticssimulation/thread/13fb26c8-75fe-43b9-9e65-6a915e7d2560 :
In RDS 2008 there is some discussion of the Generic Differential Drive in the Help file.
AllStop should cause the current operation to be cancelled, as you expect.
It will then disable the drive and you will have to issue an EnableDrive request before you can get the robot to move again.
This is by design so that AllStop can act as an Emergency Stop and prevent any further action without an explicit reset.
There should be a notification from the drive service that the drive power has been set to zero after the AllStop.
*/
#if TRACELOG
LogInfo("TrackRoamerDriveService:: AllStopHandler()");
#endif
cancelCurrentOperation();
drive.SetDrivePower zeroPower = new drive.SetDrivePower();
zeroPower.Body = new drive.SetDrivePowerRequest(0.0d, 0.0d);
zeroPower.ResponsePort = allStop.ResponsePort;
_mainPort.Post(zeroPower);
pollEncoderState(); // get starting encoder state
yield break;
}
public void driveCompletionHandler(drive.DriveStage[] driveStages)
{
#if TRACELOG
//Tracer.Trace("++++++++++++++++++ TrackRoamerDrive:: driveCompletionHandler()");
//foreach (drive.DriveStage driveStage in driveStages)
//{
// Tracer.Trace("++++++++++++++++++ driveStage=" + driveStage);
//}
#endif
pollEncoderState();
}
// Update our internal state using a drive state
void DriveStateUpdate(drive.DriveDifferentialTwoWheelState s)
{
float w = s.LeftWheel.MotorState.Pose.Orientation.W;
_state.DriveState = s;
_state.Velocity = (VelocityFromWheel(s.LeftWheel) + VelocityFromWheel(s.RightWheel)) / 2;
// Make sure that the pose is up to date
// NOTE: This is a FUDGE! Also, it only gets updated when something
// changes, not continuously, so it is usually OUT OF DATE!
_state.X = (s.LeftWheel.MotorState.Pose.Position.X +
s.RightWheel.MotorState.Pose.Position.X) / 2;
_state.Y = (s.LeftWheel.MotorState.Pose.Position.Z +
s.RightWheel.MotorState.Pose.Position.Z) / 2;
_state.Theta = (float)(Math.Acos(w) * 2 * 180 / Math.PI);
if (s.LeftWheel.MotorState.Pose.Orientation.Y < 0)
_state.Theta = -_state.Theta;
}
/// <summary>
/// Checks if autonomous obstacle avoidance should proceed
/// </summary>
/// <param name="pendingOperation">Pending drive command</param>
/// <returns>False if no pending drive request or last request was for stop motion</returns>
private bool CheckIfValidPendingDriveRequest(gendrive.SetDrivePowerRequest pendingOperation)
{
if (pendingOperation == null ||
(pendingOperation.LeftWheelPower == 0 &&
pendingOperation.RightWheelPower == 0))
{
// nothing to do. No motion
this.state.Controller.Reset();
return false;
}
return true;
}
public void HandleSubscribe(gendrive.Subscribe subscribe)
{
this.SubscribeHelper(this.submgr, subscribe, subscribe.ResponsePort);
}
public IEnumerator<ITask> SubscribeHandler(drive.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> GetHandler(drive.Get get)
{
get.ResponsePort.Post(_state);
yield break;
}
public IEnumerator<ITask> UpdateHandler(drive.Update update)
{
_state = update.Body;
_state.TimeStamp = DateTime.Now;
update.ResponsePort.Post(new DefaultUpdateResponseType());
yield break;
}
/// <summary>
/// Internal drive distance operation handler
/// </summary>
/// <param name="driveDistance"></param>
/// <returns></returns>
public virtual IEnumerator<ITask> InternalDriveDistanceHandler(drive.DriveDistance driveDistance)
{
#if TRACELOG
Tracer.Trace("InternalDriveDistanceHandler() - DriveStage." + driveDistance.Body.DriveDistanceStage);
#endif
switch (driveDistance.Body.DriveDistanceStage)
{
case drive.DriveStage.InitialRequest:
cancelCurrentOperation();
pendingDriveDistance = driveDistance; // we will need it for possible cancelation
// _state.InternalPendingDriveOperation = drive.DriveRequestOperation.DriveDistance; - not available in Proxy
_internalPendingDriveOperation = drive.DriveRequestOperation.DriveDistance;
SpawnIterator<double, double>(driveDistance.Body.Distance, driveDistance.Body.Power, DriveUntilDistance);
break;
case drive.DriveStage.Started:
SendNotification<drive.DriveDistance>(_subMgrPort, driveDistance.Body);
break;
case drive.DriveStage.Completed:
pendingDriveDistance = null;
// _state.InternalPendingDriveOperation = drive.DriveRequestOperation.NotSpecified; - not available in Proxy
_internalPendingDriveOperation = drive.DriveRequestOperation.NotSpecified;
SendNotification<drive.DriveDistance>(_subMgrPort, driveDistance.Body);
break;
case drive.DriveStage.Canceled:
pendingDriveDistance = null;
// _state.InternalPendingDriveOperation = drive.DriveRequestOperation.NotSpecified; - not available in Proxy
_internalPendingDriveOperation = drive.DriveRequestOperation.NotSpecified;
SendNotification<drive.DriveDistance>(_subMgrPort, driveDistance.Body);
break;
}
yield break;
}
/// <summary>
/// Handles Update notification from the Drive partner
/// </summary>
/// <remarks>Posts a <typeparamref name="DriveUpdate"/> request to itself.</remarks>
/// <param name="update">notification</param>
void DriveUpdateNotification(drive.Update update)
{
//LogInfo("****************************** DriveBehaviorServiceBase:: DriveUpdateNotification: state.IsEnabled=" + update.Body.IsEnabled);
_mainPort.Post(new DriveUpdate(update.Body));
}
/// <summary>
/// Internal rotate degrees handler
/// </summary>
/// <param name="rotateDegrees"></param>
/// <returns></returns>
public virtual IEnumerator<ITask> InternalRotateDegreesHandler(drive.RotateDegrees rotateDegrees)
{
#if TRACELOG
Tracer.Trace("InternalRotateDegreesHandler() - DriveStage." + rotateDegrees.Body.RotateDegreesStage);
#endif
switch (rotateDegrees.Body.RotateDegreesStage)
{
case drive.DriveStage.InitialRequest:
cancelCurrentOperation();
pendingRotateDegrees = rotateDegrees;
// _state.InternalPendingDriveOperation = drive.DriveRequestOperation.RotateDegrees; - not available in Proxy
_internalPendingDriveOperation = drive.DriveRequestOperation.RotateDegrees;
SpawnIterator<double, double>(rotateDegrees.Body.Degrees, rotateDegrees.Body.Power, RotateUntilDegrees);
break;
case drive.DriveStage.Started:
SendNotification<drive.RotateDegrees>(_subMgrPort, rotateDegrees.Body);
break;
case drive.DriveStage.Completed:
pendingRotateDegrees = null;
// _state.InternalPendingDriveOperation = drive.DriveRequestOperation.NotSpecified; - not available in Proxy
_internalPendingDriveOperation = drive.DriveRequestOperation.NotSpecified;
SendNotification<drive.RotateDegrees>(_subMgrPort, rotateDegrees.Body);
break;
case drive.DriveStage.Canceled:
pendingRotateDegrees = null;
// _state.InternalPendingDriveOperation = drive.DriveRequestOperation.NotSpecified; - not available in Proxy
_internalPendingDriveOperation = drive.DriveRequestOperation.NotSpecified;
SendNotification<drive.RotateDegrees>(_subMgrPort, rotateDegrees.Body);
break;
}
yield break;
}
public IEnumerator<ITask> GetHandler(diffdrive.Get get)
{
UpdateStateFromSimulation();
get.ResponsePort.Post(_state);
yield break;
}
public IEnumerator<ITask> ResetEncodersHandler(drive.ResetEncoders resetEncoders)
{
encoder.Reset rightResetRequest = null;
encoder.Reset leftResetRequest = null;
if (this._rightEncoderPort != null)
{
rightResetRequest = new encoder.Reset();
this._rightEncoderPort.Post(rightResetRequest);
}
if (this._leftEncoderPort != null)
{
leftResetRequest = new encoder.Reset();
this._leftEncoderPort.Post(leftResetRequest);
}
if (rightResetRequest != null)
{
yield return rightResetRequest.ResponsePort.Choice(EmptyHandler, EmptyHandler);
}
if (leftResetRequest != null)
{
yield return leftResetRequest.ResponsePort.Choice(EmptyHandler, EmptyHandler);
}
this.SendNotification<drive.ResetEncoders>(this._subMgrPort, resetEncoders.Body);
resetEncoders.ResponsePort.Post(new DefaultUpdateResponseType());
yield break;
}
public IEnumerator<ITask> RotateHandler(diffdrive.RotateDegrees rotate)
{
// TT - Added code to support RotateDegrees
// Don't waste time if the angle is zero
if (rotate.Body.Degrees != 0)
_entity.StartTurn(rotate.Body.Degrees, rotate.Body.Power);
UpdateStateFromSimulation();
rotate.ResponsePort.Post(new DefaultUpdateResponseType());
yield break;
}
public IEnumerator<ITask> RotateDegreesHandler(drive.RotateDegrees rotateDegrees)
{
ValidateDriveConfiguration(true);
if (_state.DistanceBetweenWheels <= 0)
{
rotateDegrees.ResponsePort.Post(new Fault());
throw new InvalidOperationException("The wheel encoders are not properly configured");
}
else
{
#if TRACELOG
Tracer.Trace("^^^^^^^^^^^^^^^^^^^^^ TrackRoamerDriveService:: RotateDegreesHandler(degrees=" + rotateDegrees.Body.Degrees + ")");
#endif
_state.TimeStamp = DateTime.Now;
// send immediate response
rotateDegrees.ResponsePort.Post(DefaultUpdateResponseType.Instance);
// post request to internal port.
_internalDriveOperationsPort.Post(rotateDegrees);
}
yield break;
}
public IEnumerator<ITask> SetPowerHandler(diffdrive.SetDrivePower setPower)
{
if (_entity == null)
throw new InvalidOperationException("Simulation entity not registered with service");
// Call simulation entity method for setting wheel torque
_entity.SetMotorTorque(
(float)(setPower.Body.LeftWheelPower),
(float)(setPower.Body.RightWheelPower));
UpdateStateFromSimulation();
setPower.ResponsePort.Post(DefaultUpdateResponseType.Instance);
// send update notification for entire state
_subMgrPort.Post(new submgr.Submit(_state, DsspActions.UpdateRequest));
yield break;
}
public IEnumerator<ITask> SetDrivePowerHandler(drive.SetDrivePower setDrivePower)
{
ValidateDriveConfiguration(false);
#if TRACELOG
Tracer.Trace("TR Drive: SetDrivePowerHandler() =============================== TR handler ==================================== left=" + setDrivePower.Body.LeftWheelPower + " right=" + setDrivePower.Body.RightWheelPower);
#endif
cancelCurrentOperation();
_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(new motor.SetMotorPowerRequest() { TargetPower = setDrivePower.Body.LeftWheelPower });
leftPower.ResponsePort = responsePort;
leftPower.AddHeader(coordination);
_leftMotorPort.Post(leftPower);
motor.SetMotorPower rightPower = new motor.SetMotorPower(new motor.SetMotorPowerRequest() { TargetPower = 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;
}
}));
pollEncoderState();
yield break;
}
public IEnumerator<ITask> EnableHandler(diffdrive.EnableDrive enable)
{
if (_entity == null)
throw new InvalidOperationException("Simulation entity not registered with service");
_state.IsEnabled = enable.Body.Enable;
_entity.IsEnabled = _state.IsEnabled;
UpdateStateFromSimulation();
enable.ResponsePort.Post(DefaultUpdateResponseType.Instance);
// send update for entire state
_subMgrPort.Post(new submgr.Submit(_state, DsspActions.UpdateRequest));
yield break;
}
public IEnumerator<ITask> SetDriveSpeedHandler(drive.SetDriveSpeed setDriveSpeed)
{
ValidateDriveConfiguration(true);
cancelCurrentOperation();
_state.TimeStamp = DateTime.Now;
LogError("Drive speed is not implemented");
throw new NotImplementedException();
}
/// <summary>
/// Handles Update notification from the Drive partner
/// </summary>
/// <remarks>Posts a <typeparamref name="DriveUpdate"/> request to itself.</remarks>
/// <param name="update">notification</param>
void DriveUpdateNotification(drive.Update update)
{
_mainPort.Post(new DriveUpdate(update.Body));
}
public void HandleSetPower(gendrive.SetDrivePower setPower)
{
// If either wheel power is greater than the max power per wheel
// scale both wheels down so that the higher of them is equal to max power
double higherPower = Math.Max(
Math.Abs(setPower.Body.LeftWheelPower),
Math.Abs(setPower.Body.RightWheelPower));
if (higherPower > this.state.MaxPowerPerWheel)
{
double scalingFactor = higherPower / this.state.MaxPowerPerWheel;
setPower.Body.LeftWheelPower /= scalingFactor;
setPower.Body.RightWheelPower /= scalingFactor;
}
this.NormalizeRotationSpeed(setPower);
this.pendingSetDrivePower = setPower.Body;
if (this.pendingSetDrivePower.LeftWheelPower == 0 &&
this.pendingSetDrivePower.RightWheelPower == 0)
{
this.SetPowerWithAcceleration(0, 0);
this.pendingSetDrivePower = null;
}
// simple cache power request. We will apply proper control and do sensor fusion
// for obstacle avoidance, as part of our sampling loop
setPower.ResponsePort.Post(DefaultUpdateResponseType.Instance);
}
