/// <summary>
/// Service Start
/// </summary>
protected override void Start()
{
base.Start();
MainPortInterleave.CombineWith(
Arbiter.Interleave(
new TeardownReceiverGroup(),
new ExclusiveReceiverGroup
(
Arbiter.Receive <OnLoad>(true, _eventsPort, OnLoadHandler),
Arbiter.Receive <OnClosed>(true, _eventsPort, OnClosedHandler)
),
new ConcurrentReceiverGroup()
)
);
WinFormsServicePort.Post(
new RunForm(
delegate()
{
return(new VideoStreamViewerForm(_eventsPort));
}
)
);
// Add service specific initialization here.
}
/// <summary>
/// Service start
/// </summary>
protected override void Start()
{
Tracer.Trace("TrackRoamerUsrfService::Start()");
_httpUtilities = DsspHttpUtilitiesService.Create(Environment);
base.Start(); // fire up MainPortInterleave; wireup [ServiceHandler] methods
MainPortInterleave.CombineWith(new Interleave(
new ExclusiveReceiverGroup(
Arbiter.ReceiveWithIterator <sicklrf.Subscribe>(true, _mainPort, SubscribeHandler),
Arbiter.ReceiveWithIterator <sicklrf.ReliableSubscribe>(true, _mainPort, ReliableSubscribeHandler),
Arbiter.Receive <sicklrf.Get>(true, _mainPort, GetHandler),
Arbiter.Receive <sicklrf.Reset>(true, _mainPort, ResetHandler),
Arbiter.Receive <proxibrick.UpdateSonarData>(true, _trackRoamerBrickProximityBoardServiceNotify, trpbUpdateSonarNotification)
),
new ConcurrentReceiverGroup()
));
Tracer.Trace("TrackRoamerUsrfService: calling Subscribe() for UpdateSonarData");
Type[] notifyMeOf = new Type[] { typeof(proxibrick.UpdateSonarData) };
_trackRoamerBrickProximityBoardServicePort.Subscribe(_trackRoamerBrickProximityBoardServiceNotify, notifyMeOf);
}
/// <summary>
/// Service Start
/// </summary>
protected override void Start()
{
ValidateState();
base.Start();
// Find our simulation entity that represents the "hardware" or real-world service.
// To hook up with simulation entities we do the following steps
// 1) have a manifest or some other service create us, specifying a partner named SimulationEntity
// 2) in the simulation service (us) issue a subscribe to the simulation engine looking for
// an instance of that simulation entity. We use the Entity.State.Name for the match so it must be
// exactly the same. See SimulationTutorial2 for the creation process
// 3) Listen for a notification telling us the entity is available
// 4) cache reference to entity and communicate with it issuing low level commands.
_simEngine = simengine.SimulationEngine.GlobalInstancePort;
_notificationTarget = new simengine.SimulationEnginePort();
MainPortInterleave.CombineWith(new Interleave(
new TeardownReceiverGroup(),
new ExclusiveReceiverGroup
(
Arbiter.Receive <simengine.InsertSimulationEntity>(true, _notificationTarget, InsertEntityNotificationHandlerFirstTime),
Arbiter.ReceiveWithIterator <irobottypes.Connect>(true, _mainPort, ConnectHandler)
),
new ConcurrentReceiverGroup()
));
SubscribeForEntity();
}
/// <summary>
/// Service start
/// </summary>
protected override void Start()
{
base.Start();
this.InitializeState();
// Handlers that need write or exclusive access to state go under
// the exclusive group. Handlers that need read or shared access, and can be
// concurrent to other readers, go to the concurrent group.
// Other internal ports can be included in interleave so you can coordinate
// intermediate computation with top level handlers.
MainPortInterleave.CombineWith(
Arbiter.Interleave(
new TeardownReceiverGroup(),
new ExclusiveReceiverGroup(
Arbiter.ReceiveWithIterator <OnLoad>(true, this.eventsPort, this.OnLoadHandler),
Arbiter.Receive <OnClosed>(true, this.eventsPort, this.OnClosedHandler),
Arbiter.ReceiveWithIterator <OnChangeJoystick>(true, this.eventsPort, this.OnChangeJoystickHandler),
Arbiter.ReceiveWithIterator <OnChangeTilt>(true, this.eventsPort, this.OnChangeTiltHandler),
Arbiter.Receive <pantilt.Rotate>(true, this.panTiltNotify, this.OnRotateSingleAxis),
Arbiter.ReceiveWithIterator <OnOptionSettings>(true, this.eventsPort, this.OnOptionSettingsHandler),
Arbiter.Receive <webcamsensor.Replace>(true, this.webCamNotify, this.CameraUpdateFrameHandler),
Arbiter.Receive <depthcamsensor.Replace>(true, this.depthCamSensorNotify, this.DepthCameraUpdateFrameHandler)),
new ConcurrentReceiverGroup(
Arbiter.Receive <OnResetEncoders>(true, this.eventsPort, this.OnResetEncodersHandler),
Arbiter.ReceiveWithIterator <game.Replace>(true, this.gameControllerNotify, this.JoystickReplaceHandler),
Arbiter.ReceiveWithIterator <game.UpdateAxes>(true, this.gameControllerNotify, this.JoystickUpdateAxesHandler),
Arbiter.ReceiveWithIterator <game.UpdateButtons>(true, this.gameControllerNotify, this.JoystickUpdateButtonsHandler),
Arbiter.ReceiveWithIterator <OnMove>(true, this.eventsPort, this.OnMoveHandler),
Arbiter.ReceiveWithIterator <OnMotionCommand>(true, this.eventsPort, this.OnMotionCommandHandler))));
SpawnIterator(this.Setup);
}
protected override void Start()
{
// Configure default state. It will be replaced by the drive during configuration.
if (_state == null)
{
_state = new pxencoder.EncoderState();
_state.HardwareIdentifier = -1; // 1=Left 2=Right
_state.TicksPerRevolution = 100;
SaveState(_state);
}
base.Start();
MainPortInterleave.CombineWith(new Interleave(
new ExclusiveReceiverGroup(
// prepare listening for Power Brick's encoder change notifications
Arbiter.Receive <powerbrick.UpdateMotorEncoder>(true, notificationPortEncoder, MotorEncoderNotificationHandler)
),
new ConcurrentReceiverGroup()
));
// display HTTP service Uri
LogInfo("Service uri: " + ServiceInfo.HttpUri());
// Subscribe to the Hardware Controller for encoder notifications
if (ValidState(_state))
{
SubscribeToPowerBrick();
}
}
/// <summary>
/// Helper function to initialize Kinect camera.
/// </summary>
/// <returns>CCR Iterator</returns>
protected IEnumerator <ITask> InitializeKinect()
{
var finishedInitialization = new SuccessFailurePort();
ThreadPool.QueueUserWorkItem(
param =>
{
try
{
this.InitializeKinectDevice();
finishedInitialization.Post(SuccessResult.Instance);
}
catch (Exception e)
{
LogError(e);
finishedInitialization.Post(new Exception(Resources.InitializeFailed));
}
});
yield return(finishedInitialization.Choice(
success =>
{
base.Start();
this.StartPollingBehavior();
// Merge the internal update ports into the main interleave
MainPortInterleave.CombineWith(
new Interleave(
new ExclusiveReceiverGroup(
Arbiter.Receive(
true,
this.alternateContractUpdatePort,
this.OnUpdateAlternates)),
new ConcurrentReceiverGroup(
Arbiter.Receive(
true,
this.scheduleNextFrameReadPort,
this.OnScheduleNextFrameRead))));
LogInfo(Resources.Initialized);
},
fault =>
{
LogError(fault);
base.StartFailed();
}));
}
private void StartBehavior()
{
base.Start();
MainPortInterleave.CombineWith(
new Interleave(
new TeardownReceiverGroup(),
new ExclusiveReceiverGroup(
Arbiter.Receive <SensorUpdate>(true, _updatePort, SensorUpdateHandler),
Arbiter.Receive <SensorConfiguration>(true, _updatePort, SensorConfigurationHandler),
Arbiter.Receive <DateTime>(true, _timerPort, TimerHandler)
),
new ConcurrentReceiverGroup()
)
);
}
void OnFoundEntity(sime.InsertSimulationEntity insert)
{
OnInsertEntity(insert);
MainPortInterleave.CombineWith(
new Interleave(
new TeardownReceiverGroup(),
new ExclusiveReceiverGroup(
Arbiter.Receive <sime.InsertSimulationEntity>(true, _simNotify, OnInsertEntity),
Arbiter.Receive <sime.DeleteSimulationEntity>(true, _simNotify, OnDeleteEntity)
),
new ConcurrentReceiverGroup()
)
);
}
void InsertEntityNotificationHandlerFirstTime(simengine.InsertSimulationEntity ins)
{
InsertEntityNotificationHandler(ins);
base.Start();
MainPortInterleave.CombineWith(
new Interleave(
new TeardownReceiverGroup(),
new ExclusiveReceiverGroup(
Arbiter.Receive <simengine.InsertSimulationEntity>(true, _notificationTarget, InsertEntityNotificationHandler),
Arbiter.Receive <simengine.DeleteSimulationEntity>(true, _notificationTarget, DeleteEntityNotificationHandler)
),
new ConcurrentReceiverGroup()
)
);
}
/// <summary>
/// Service start
/// </summary>
protected override void Start()
{
base.Start(); // start MainPortInterleave; wireup [ServiceHandler] methods
if (_state == null)
{
_state = new AnimatedHeadServiceState();
_state.AnimatedHeadServiceConfig = new AnimatedHeadConfig();
_state.AnimatedHeadServiceConfig.CommPort = 0;
SaveState(_state);
}
else
{
// Clear old Animated Head readings
//_state.Quaternion = null;
_state.Connected = false;
}
_httpUtilities = DsspHttpUtilitiesService.Create(Environment);
if (_state.AnimatedHeadServiceConfig == null)
{
_state.AnimatedHeadServiceConfig = new AnimatedHeadConfig();
}
// Publish the service to the local Node Directory
//DirectoryInsert();
_ahConnection = new AnimatedHeadConnection(_state.AnimatedHeadServiceConfig, _ahDataPort);
SpawnIterator(ConnectToAnimatedHead);
//SpawnIterator(TestAnimatedHead);
// if we have anything coming from Arduino - the _ahDataPort Arbiter.Receive<string[]> and others must be operational:
// Listen on the main port for requests and call the appropriate handler.
MainPortInterleave.CombineWith(new Interleave(new ExclusiveReceiverGroup(
Arbiter.Receive <string[]>(true, _ahDataPort, DataReceivedHandler),
Arbiter.Receive <Exception>(true, _ahDataPort, ExceptionHandler),
Arbiter.Receive <string>(true, _ahDataPort, MessageHandler)
),
new ConcurrentReceiverGroup()));
//base.Start(); // can't have it here, we already started mainInterleave and added to directory via DirectoryInsert
}
private void StartBehavior()
{
base.Start();
MainPortInterleave.CombineWith(
Arbiter.Interleave
(
new TeardownReceiverGroup(),
new ExclusiveReceiverGroup
(
Arbiter.Receive <ActuatorConfiguration>(true, _updatePort, ActuatorConfigurationHandler),
Arbiter.Receive <InternalSetActuatorValue>(true, _updatePort, SetActuatorValueHandler)
),
new ConcurrentReceiverGroup()
)
);
}
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>
/// Initializes this instance.
/// </summary>
/// <returns>Standard ccr iterator</returns>
private IEnumerator <ITask> Initialize()
{
var located = new PortSet <VisualEntity, Fault>();
SpawnIterator(located, this.LocateCameraEntity);
yield return(located.Choice());
var entity = (VisualEntity)located;
if (entity == null)
{
LogError("Kinect entity not found");
StartFailed();
yield break;
}
this.kinectEntity = (KinectEntity)entity;
if (this.state == null)
{
this.state = new kinect.KinectState
{
DepthImageFormat = DepthImageFormat.Resolution320x240Fps30,
FrameRate = MaxFramesPerSec,
IsDepthServiceUpdateEnabled = true,
IsWebCamServiceUpdateEnabled = true,
UseColor = true,
UseDepth = true,
UseSkeletalTracking = false,
ColorImageFormat = ColorImageFormat.RgbResolution640x480Fps30
};
SaveState(this.state);
}
this.panTiltState = InitialPanTiltState();
base.Start();
MainPortInterleave.CombineWith(
new Interleave(
new ExclusiveReceiverGroup(
Arbiter.ReceiveWithIterator(true, this.pollPort, this.DrainPendingRequests)),
new ConcurrentReceiverGroup()));
this.kinectOps.Post(new kinect.SetFrameRate(new kinect.SetFrameRateRequest(this.state.FrameRate)));
}
// Complete the initialization
void StartComplete()
{
base.Start();
#region Activate TTS Notification Handler
// activate a handler for viseme notifications
MainPortInterleave.CombineWith(
Arbiter.Interleave(
new TeardownReceiverGroup(),
new ExclusiveReceiverGroup(
Arbiter.Receive <texttospeech.VisemeNotify>(true, _ttsNotifications, VisemeNotifyHandler)
),
new ConcurrentReceiverGroup()
)
);
#endregion
}
void InsertEntityNotificationHandlerFirstTime(simengine.InsertSimulationEntity ins)
{
InsertEntityNotificationHandler(ins);
base.Start();
// Listen on the main port for requests and call the appropriate handler.
MainPortInterleave.CombineWith(
new Interleave(
new TeardownReceiverGroup(),
new ExclusiveReceiverGroup(
Arbiter.Receive <simengine.InsertSimulationEntity>(true, _notificationTarget, InsertEntityNotificationHandler),
Arbiter.Receive <simengine.DeleteSimulationEntity>(true, _notificationTarget, DeleteEntityNotificationHandler),
Arbiter.Receive <FromWinformMsg>(true, _fromWinformPort, OnWinformMessageHandler)
),
new ConcurrentReceiverGroup()
)
);
}
/// <summary>
/// Service start
/// </summary>
protected override void Start()
{
base.Start();
// Needed for HttpPost
this.httpUtilities = DsspHttpUtilitiesService.Create(Environment);
// Register handlers for notification from speech recognizer
MainPortInterleave.CombineWith(new Interleave(
new ExclusiveReceiverGroup(
Arbiter.Receive <sr.Replace>(true, this.speechRecoNotifyPort, this.SpeechRecognizerReplaceHandler),
Arbiter.Receive <sr.SpeechDetected>(true, this.speechRecoNotifyPort, this.SpeechDetectedHandler),
Arbiter.Receive <sr.SpeechRecognized>(true, this.speechRecoNotifyPort, this.SpeechRecognizedHandler),
Arbiter.Receive <sr.SpeechRecognitionRejected>(true, this.speechRecoNotifyPort, this.SpeechRecognitionRejectedHandler),
Arbiter.Receive <sr.BeamDirectionChanged>(true, this.speechRecoNotifyPort, this.BeamDirectionChangedHandler)),
new ConcurrentReceiverGroup()));
this.speechRecoPort.Subscribe(this.speechRecoNotifyPort);
}
/// <summary>
/// Service start
/// </summary>
protected override void Start()
{
base.Start();
// combine with our main coordination
MainPortInterleave.CombineWith(
Arbiter.Interleave(
new TeardownReceiverGroup(),
new ExclusiveReceiverGroup(
Arbiter.Receive <FromServerUiMsg>(true, _fromServerUiPort, OnServerUiMessageHandler),
Arbiter.Receive <ClientBase.ClientAnnounceDecision>(true, _clientBasePort, ClientAnnounceDecisionHandler) //added 20110110
//Arbiter.Receive<FromServerUiControlButtonMsg>(true, _fromServerUiPort, OnServerUiControlButtonMsgHandler)
),
new ConcurrentReceiverGroup(
)
)
);
WinFormsServicePort.Post(new RunForm(CreateForm)); // 显示服务端界面
}
/// <summary>
/// Service Start
/// </summary>
#region CODECLIP 03-1
protected override void Start()
{
base.Start();
// Add the handlers for notifications from ServiceTutorial4.
// This is necessary because these handlers do not handle
// operations in this service, so you cannot mark them with
// the ServiceHandler attribute.
MainPortInterleave.CombineWith(
new Interleave(
new TeardownReceiverGroup(),
new ExclusiveReceiverGroup(),
new ConcurrentReceiverGroup(
Arbiter.Receive <rst4.IncrementTick>(true, _clockNotify, NotifyTickHandler),
Arbiter.Receive <rst4.Replace>(true, _clockNotify, NotifyReplaceHandler)
))
);
#region CODECLIP 02-3
_clockPort.Subscribe(_clockNotify);
#endregion
}
/// <summary>
/// Service Start
/// </summary>
protected override void Start()
{
ValidateState();
// Listen on the main port for requests and call the appropriate handler.
ActivateDsspOperationHandlers();
// Publish the service to the local service Directory
DirectoryInsert();
MainPortInterleave.CombineWith(new Interleave(
new ExclusiveReceiverGroup(),
new ConcurrentReceiverGroup(
Arbiter.Receive <sensorupdates.UpdateBumpsCliffsAndWalls>(true, _iRobotSensorUpdates, ReturnSensorsHandler)
)));
// Subscribe to the iRobot sensors
_iRobotLitePort.Subscribe(_iRobotSensorUpdates, typeof(sensorupdates.UpdateBumpsCliffsAndWalls));
// display HTTP service Uri
LogInfo(LogGroups.Console, "Service uri: ");
}
/// <summary>
/// Service start method
/// </summary>
protected override void Start()
{
if (_state == null)
{
_state = new IpCameraState();
}
_utilitiesPort = DsspHttpUtilitiesService.Create(Environment);
Uri httpAlias = ServiceInfo.HttpServiceAlias;
if (httpAlias != null)
{
_prefix = httpAlias.AbsolutePath;
_alternatePrefix = AlternateContractServiceInfo[0].HttpServiceAlias.AbsolutePath;
}
else
{
LogError(LogGroups.Activation, "Service requires HTTP transport");
DefaultDropHandler(new DsspDefaultDrop());
return;
}
base.Start();
MainPortInterleave.CombineWith(
new Interleave(
new ExclusiveReceiverGroup(
Arbiter.Receive <Bitmap>(true, _imagePort, ImageHandler)
),
new ConcurrentReceiverGroup(
Arbiter.Receive <DateTime>(true, _imagePort, GetImageHandler)
)
)
);
_imagePort.Post(DateTime.UtcNow);
}
/// <summary>
/// Service start
/// </summary>
protected override void Start()
{
base.Start();
//_serverNotificationPort = ServiceForwarder<server.Sim2DSvrOperations>(_strServerUri);
// combine with our main coordination
MainPortInterleave.CombineWith(
Arbiter.Interleave(
new TeardownReceiverGroup(),
new ExclusiveReceiverGroup(
Arbiter.Receive <FromClientUiMsg>(true, _fromClientUiPort, OnClientUiMessageHandler)
),
new ConcurrentReceiverGroup(
Arbiter.Receive <server.CompetitionTypeChanged>(true, _serverNotificationNotify, NotifyCompetitionTypeHandler),
Arbiter.Receive <server.CompetitionControlButton>(true, _serverNotificationNotify, CompetitionControlButtonNotificationHandler),
Arbiter.Receive <server.MissionPara>(true, _serverNotificationNotify, MissionParaNotificationHandler),
Arbiter.Receive <client.HeartBeat>(true, _clientBasePort, HeartBeatAnnounceHandler),
Arbiter.Receive <client.TeamIdChanged>(true, _clientBasePort, TeamIdChangedAnnounceHandler)
)
)
);
WinFormsServicePort.Post(new RunForm(CreateForm)); // 显示客户端界面
// 从Manifest文件获取Server服务实例的Uri
PartnerType partnerTypeServer = FindPartner("Server");
if (partnerTypeServer != null)
{
_strServerUri = partnerTypeServer.Service;
_serverNotificationPort = ServiceForwarder <server.Sim2DSvrOperations>(_strServerUri);
}
//server.Subscribe subscribe;
//_serverNotificationPort.Subscribe(_serverNotificationNotify, out subscribe);
}
protected override void Start()
{
LogInfo("TrackRoamerBrickPowerService:Start() _state=" + _state);
if (_state == null)
{
// we usually don't come here, as the state is already configured by file - TrackRoamer.TrackRoamerBrickPower.Bot.Config.xml
LogInfo("TrackRoamerBrickPowerService:Start(): _state == null - initializing...");
_state = new TrackRoamerBrickPowerState();
_state.PowerControllerConfig = new PowerControllerConfig();
_state.PowerControllerConfig.CommPort = DEFAULT_COM_PORT_NUMBER;
_state.Connected = false;
_state.FrameCounter = 0;
_state.Whiskers = new Whiskers();
_state.MotorSpeed = new MotorSpeed();
_state.MotorEncoder = new MotorEncoder();
_state.MotorEncoderSpeed = new MotorEncoderSpeed();
_state.TimeStamp = DateTime.Now;
SaveState(_state);
}
else
{
LogInfo("TrackRoamerBrickPowerService:Start(): _state is supplied by file: " + _configFile);
}
if (_state.PowerControllerConfig == null)
{
_state.PowerControllerConfig = new PowerControllerConfig();
}
_brickConnection = new BrickConnection(_state.PowerControllerConfig, _rbtqDataPort, this);
// wireup all event handlers to receive AX2850 data:
_brickConnection.onValueReceived_EncoderLeftAbsolute += new OnValueReceived(_brickConnection_onValueReceived_EncoderLeftAbsolute);
_brickConnection.onValueReceived_EncoderRightAbsolute += new OnValueReceived(_brickConnection_onValueReceived_EncoderRightAbsolute);
_brickConnection.onValueReceived_EncoderSpeed += new OnValueReceived(_brickConnection_onValueReceived_EncoderSpeed);
_brickConnection.onValueReceived_WhiskerLeft += new OnValueReceived(onWhiskerLeft);
_brickConnection.onValueReceived_WhiskerRight += new OnValueReceived(onWhiskerRight);
_brickConnection.onValueReceived_AnalogInputs += new OnValueReceived(_brickConnection_onValueReceived_AnalogInputs);
_brickConnection.onValueReceived_DigitalInputE += new OnValueReceived(_brickConnection_onValueReceived_DigitalInputE);
_brickConnection.onValueReceived_Voltage += new OnValueReceived(_brickConnection_onValueReceived_Voltage);
_brickConnection.onValueReceived_MotorPower += new OnValueReceived(_brickConnection_onValueReceived_MotorPower);
_brickConnection.onValueReceived_MotorAmps += new OnValueReceived(_brickConnection_onValueReceived_MotorAmps);
_brickConnection.onValueReceived_HeatsinkTemperature += new OnValueReceived(_brickConnection_onValueReceived_HeatsinkTemperature);
base.Start(); // start MainPortInterleave; wireup [ServiceHandler] methods
SpawnIterator(ConnectToPowerController);
MainPortInterleave.CombineWith(
Arbiter.Interleave(
new TeardownReceiverGroup(),
new ExclusiveReceiverGroup(
Arbiter.ReceiveWithIterator(true, _controlPort, ControlLoop),
Arbiter.Receive <Exception>(true, _rbtqDataPort, ExceptionHandler),
Arbiter.Receive <string>(true, _rbtqDataPort, MessageHandler)
),
new ConcurrentReceiverGroup()));
// kick off control loop interval:
_controlPort.Post(DateTime.UtcNow);
lastRateSnapshot = DateTime.Now.AddSeconds(frameRateWatchdogDelaySec); // delay frame rate watchdog
LogInfo("OK: TrackRoamerBrickPowerService:ControlLoop activated");
// display HTTP service Uri
LogInfo("TrackRoamerBrickPowerService: Service uri: ");
}
/// <summary>
/// Tries to subscribe to an auto partner single contract, or throws an exception
/// if we don't support the contract. Also starts a 1-second poll that continues
/// until publishers send non-zero-length arrays.
/// </summary>
/// <param name="def"></param>
/// <param name="serviceInfo"></param>
private void subscribeAutoSingle(AutoDefinition def, ServiceInfoType serviceInfo)
{
// Check for each contract we know about and subscribe.
////////////////// Analog Sensor
if (serviceInfo.Contract.Equals(analog.Contract.Identifier))
{
var partnerPort = ServiceForwarder <analog.AnalogSensorOperations>(new Uri(serviceInfo.Service));
var notifyPort = new Port <analog.Replace>();
Activate(Arbiter.Choice(partnerPort.Subscribe(notifyPort, typeof(analog.Replace)),
delegate(SubscribeResponseType success)
{
MainPortInterleave.CombineWith(new Interleave(
new ExclusiveReceiverGroup(
Arbiter.Receive(true, notifyPort,
delegate(analog.Replace replace)
{ autoSubscribeNotificationHelper(def, new List <double>(1)
{
replace.Body.RawMeasurement
}, replace.Body.TimeStamp, false); }
)),
new ConcurrentReceiverGroup()));
},
delegate(Fault failure)
{
LogError("Fault while subscribing to auto partner", failure);
}));
}
////////////////// Analog Sensor Array
else if (serviceInfo.Contract.Equals(analogArray.Contract.Identifier))
{
var partnerPort = ServiceForwarder <analogArray.AnalogSensorOperations>(new Uri(serviceInfo.Service));
var notifyPort = new Port <analogArray.Replace>();
var pollPort = base.TimeoutPort(1000);
bool gotNonzeroState = false;
Activate(Arbiter.Receive(true, pollPort,
delegate(DateTime time)
{
if (gotNonzeroState == false)
{
Activate(Arbiter.Choice(partnerPort.Get(),
delegate(analogArray.AnalogSensorArrayState state) { notifyPort.Post(new analogArray.Replace(state)); },
delegate(Fault failure) { LogError("Vector got fault while trying to get polled state", failure); }));
TaskQueue.EnqueueTimer(TimeSpan.FromMilliseconds(1000.0), pollPort);
}
}));
Activate(Arbiter.Choice(partnerPort.Subscribe(notifyPort, typeof(analogArray.Replace)),
delegate(SubscribeResponseType success)
{
MainPortInterleave.CombineWith(new Interleave(
new ExclusiveReceiverGroup(
Arbiter.Receive(true, notifyPort,
delegate(analogArray.Replace replace)
{
if (replace.Body.Sensors.Count > 0)
{
gotNonzeroState = true;
}
autoSubscribeNotificationHelper(def,
new List <double>(from s in replace.Body.Sensors select s.RawMeasurement),
(replace.Body.Sensors.Count > 0 ? replace.Body.Sensors[0].TimeStamp : DateTime.Now), false);
})),
new ConcurrentReceiverGroup()));
},
delegate(Fault failure)
{
LogError("Fault while subscribing to auto partner", failure);
}));
}
////////////////// Contact Sensor
else if (serviceInfo.Contract.Equals(contact.Contract.Identifier))
{
var partnerPort = ServiceForwarder <contact.ContactSensorArrayOperations>(new Uri(serviceInfo.Service));
var notifyPort = new PortSet <contact.Replace, contact.Update>();
// Post an initial Replace notification to update the HW ID map (no, this is now done automatically)
//notifyPort.Post(new contact.Replace(RSUtils.ReceiveSync(TaskQueue, partnerPort.Get(), Params.defaultRecieveTimeout)));
var pollPort = base.TimeoutPort(1000);
bool gotNonzeroState = false;
Activate(Arbiter.Receive(true, pollPort,
delegate(DateTime time)
{
if (gotNonzeroState == false)
{
Activate(Arbiter.Choice(partnerPort.Get(),
delegate(contact.ContactSensorArrayState state) { notifyPort.Post(new contact.Replace(state)); },
delegate(Fault failure) { LogError("Vector got fault while trying to get polled state", failure); }));
TaskQueue.EnqueueTimer(TimeSpan.FromMilliseconds(1000.0), pollPort);
}
}));
Activate(Arbiter.Choice(partnerPort.Subscribe(notifyPort, typeof(contact.Replace), typeof(contact.Update)),
delegate(SubscribeResponseType success)
{
var contactHWIDMap = new Dictionary <int, int>();
MainPortInterleave.CombineWith(new Interleave(
new ExclusiveReceiverGroup(
Arbiter.Receive <contact.Replace>(true, notifyPort,
delegate(contact.Replace replace)
{
if (replace.Body.Sensors.Count > 0)
{
gotNonzeroState = true;
}
if (def.hwKeys.Count < replace.Body.Sensors.Count)
{
def.hwKeys.Capacity = replace.Body.Sensors.Count;
while (def.hwKeys.Count < replace.Body.Sensors.Count)
{
def.hwKeys.Add("");
}
}
// Refresh the HW ID map
contactHWIDMap.Clear();
for (int i = 0; i < replace.Body.Sensors.Count; i++)
{
contactHWIDMap.Add(replace.Body.Sensors[i].HardwareIdentifier, i);
//if (def.hwKeys[i] == null || def.hwKeys[i].Length <= 0)
def.hwKeys[i] = replace.Body.Sensors[i].Name;
}
autoSubscribeNotificationHelper(def,
new List <double>(from s in replace.Body.Sensors select(s.Pressed ? 1.0 : 0.0)),
(replace.Body.Sensors.Count > 0 ? replace.Body.Sensors[0].TimeStamp : DateTime.Now), true);
}),
Arbiter.Receive <contact.Update>(true, notifyPort,
delegate(contact.Update update)
{
gotNonzeroState = true;
try
{
int i = contactHWIDMap[update.Body.HardwareIdentifier];
autoSubscribeNotificationUpdateHelper(def, i, (update.Body.Pressed ? 1.0 : 0.0), update.Body.TimeStamp);
}
catch (KeyNotFoundException)
{
LogError("Vector got update for contact sensor hardware identifier " + update.Body.HardwareIdentifier + " and doesn't know about this hardware ID. Trying to fetch entire state.");
Activate(Arbiter.Choice(partnerPort.Get(),
delegate(contact.ContactSensorArrayState state) { notifyPort.Post(new contact.Replace(state)); },
delegate(Fault failure) { LogError("Vector got fault while trying to get contact state", failure); }));
}
})),
new ConcurrentReceiverGroup()));
},
delegate(Fault failure)
{
LogError("Fault while subscribing to auto partner", failure);
}));
}
else
{
throw new UnrecognizedContractException();
}
}
/// <summary>
/// Allocation and assignments for first run
/// </summary>
/// <returns>True if initialization succeeded, otherwise False</returns>
private bool Initialize()
{
if (this.initialized)
{
return(this.initialized);
}
try
{
// No persisted state file, create a new one
if (this.state == null)
{
this.state = new MarkRobotState();
}
// Populate the IR sensor state
if (this.state.InfraredSensorState == null)
{
this.state.InfraredSensorState = new ir.InfraredSensorArrayState();
}
if (this.state.InfraredSensorState.Sensors == null)
{
this.state.InfraredSensorState.Sensors = new List <Services.Infrared.InfraredState>();
}
if (this.state.InfraredSensorState.Sensors.Count == 0)
{
foreach (int irIdentifier in DefaultInfraredArrayIdentifiers)
{
this.state.InfraredSensorState.Sensors.Add(new Services.Infrared.InfraredState()
{
HardwareIdentifier = irIdentifier
});
}
}
// Populate the Sonar sensor state
if (this.state.SonarSensorState == null)
{
this.state.SonarSensorState = new sonar.SonarSensorArrayState();
}
if (this.state.SonarSensorState.Sensors == null)
{
this.state.SonarSensorState.Sensors = new List <Services.Sonar.SonarState>();
}
if (this.state.SonarSensorState.Sensors.Count == 0)
{
foreach (int sonarIdentifier in DefaultSonarArrayIdentifiers)
{
this.state.SonarSensorState.Sensors.Add(new Services.Sonar.SonarState()
{
HardwareIdentifier = sonarIdentifier
});
}
}
this.state.DriveState = new drive.DriveDifferentialTwoWheelState();
if (this.state.BatteryState == null)
{
this.state.BatteryState = new Services.Battery.BatteryState();
}
if (this.state.BatteryState.MaxBatteryPower == 0)
{
this.state.BatteryState.MaxBatteryPower = DefaultMaxBatteryPower;
}
}
catch (Exception e)
{
LogError(e);
this.Shutdown();
return(false);
}
this.state.LastStartTime = DateTime.Now;
SaveState(this.state);
base.Start();
// Make sure the pin polling port is in the main interleave because it modifies service state
MainPortInterleave.CombineWith(
new Interleave(
new ExclusiveReceiverGroup(
Arbiter.ReceiveWithIterator(true, this.sensorPollingPort, this.PollSensors)),
new ConcurrentReceiverGroup(
Arbiter.Receive <drive.Update>(true, this.driveNotifyPort, this.DriveNotification))));
this.controllerDrivePort.Post(new drive.Subscribe()
{
NotificationPort = this.driveNotifyPort
});
this.controllerDrivePort.Post(new drive.ReliableSubscribe()
{
NotificationPort = this.driveNotifyPort
});
// Start the pin polling interval
this.sensorPollingPort.Post(DateTime.Now);
return(true);
}
/// <summary>
/// Retrieve the COM port specified in the SerialCOMService state.
/// Perform initialization if COM port is correct and available.
/// </summary>
/// <returns>Enumerator of type ITask</returns>
private IEnumerator <ITask> InternalInitialize()
{
// Make sure we have a valid and open COM port
int currentCOMPort = 0;
yield return(this.serialCOMServicePort.GetConfiguration().Choice(
s => currentCOMPort = s.PortNumber,
f => LogError("Failed to retrieve config from Serial Port partner service")));
yield return(this.serialCOMServicePort.OpenPort().Choice(
s => LogInfo(string.Format("Opened COM{0}", currentCOMPort)),
f => LogError(string.Format("Failed to open COM{0}", currentCOMPort))));
if (currentCOMPort == 0)
{
LogError("Parallax2011ReferencePlatformIoController Service failed to initialize: Check 'PortNumber' in serialcomservice.config.xml");
this.Shutdown();
yield break;
}
this.initialized = this.Initialize();
if (this.initialized)
{
// Make sure we have a live Parallax board on the COM port by retrieving the FW version string
serialcomservice.SendAndGetRequest sg = new serialcomservice.SendAndGetRequest();
sg.Timeout = this.state.DefaultResponsePause;
sg.Terminator = board.PacketTerminatorString;
sg.Data = new serialcomservice.Packet(board.CreatePacket <byte>(board.GetVersionString));
PortSet <serialcomservice.Packet, soap.Fault> resultPort = this.serialCOMServicePort.SendAndGet(sg);
yield return(resultPort.Choice());
soap.Fault f = (soap.Fault)resultPort;
if (f == null)
{
serialcomservice.Packet p = (serialcomservice.Packet)resultPort;
if (p != null)
{
if (p.Message != null)
{
string str = Encoding.ASCII.GetString(p.Message);
this.state.FWVersion = Convert.ToInt32(str, 16);
}
}
}
else
{
LogError(string.Format("Failed to send command: {0}", Encoding.ASCII.GetString(sg.Data.Message)));
LogError("Failed to receive FW version!");
}
LogInfo(string.Format("FW Version: {0}", this.state.FWVersion));
serialcomservice.SendAndGetRequest accSendAndGet = new serialcomservice.SendAndGetRequest();
accSendAndGet.Timeout = this.state.DefaultResponsePause;
accSendAndGet.Terminator = board.PacketTerminatorString;
accSendAndGet.Data = new serialcomservice.Packet(board.CreatePacket <ushort>(board.SetRampingValueString, (ushort)this.state.AccelerationRate));
resultPort = this.serialCOMServicePort.SendAndGet(accSendAndGet);
yield return(resultPort.Choice());
f = (soap.Fault)resultPort;
if (f != null)
{
LogError(string.Format("Failed to send command: {0}", Encoding.ASCII.GetString(accSendAndGet.Data.Message)));
LogError("Failed to set acceleration!");
}
this.encoderTicksPerMeter = new double[]
{
this.state.DriveState.LeftWheel.EncoderState.TicksPerRevolution / (2 * this.state.DriveState.LeftWheel.Radius * Math.PI),
this.state.DriveState.RightWheel.EncoderState.TicksPerRevolution / (2 * this.state.DriveState.RightWheel.Radius * Math.PI)
};
SaveState(this.state);
base.Start();
// Make sure the pin polling port is in the main interleave because it modifies service state
MainPortInterleave.CombineWith(
new Interleave(
new ExclusiveReceiverGroup(
Arbiter.ReceiveWithIterator(true, this.pinPollingPort, this.PollPins)),
new ConcurrentReceiverGroup()));
// Start the pin polling interval
this.pinPollingPort.Post(DateTime.Now);
}
}
/// <summary>
/// This handler receives an announcement from the simulation engine that
/// contains a pointer to the entity associated with this service.
/// </summary>
/// <param name="ins"></param>
void InsertEntityNotificationHandlerFirstTime(simengine.InsertSimulationEntity ins)
{
_entity = (simengine.IRobotCreate)ins.Body;
_entity.ServiceContract = Contract.Identifier;
_contactPort = new Port <simcommon.EntityContactNotification>();
// add the contact handler
MainPortInterleave.CombineWith(new Interleave(
new TeardownReceiverGroup(
),
new ExclusiveReceiverGroup
(
),
new ConcurrentReceiverGroup
(
Arbiter.Receive <simcommon.EntityContactNotification>(true, _contactPort, ContactHandler)
)
));
_entity.Subscribe(_contactPort);
PartnerType irEntityPartner = Dss.ServiceModel.DsspServiceBase.DsspServiceBase.FindPartner(
new System.Xml.XmlQualifiedName("IREntity", Contract.Identifier), ServiceInfo.PartnerList);
// only add an IR Entity if one was requested
if (irEntityPartner != null)
{
// add an IR sensor, if necessary
_irEntity = null;
foreach (simengine.VisualEntity child in _entity.Children)
{
if (child.GetType() == typeof(IREntity))
{
_irEntity = (IREntity)child;
}
}
if (_irEntity == null)
{
// didn't find on already there so add one
// irentity is on the right front quadrant of the Create facing to the right
_irEntity = new IREntity(_entity.State.Name + "_IR", new Pose(
new Vector3(0.115f, 0.055f, -0.115f),
simengine.TypeConversion.FromXNA(xna.Quaternion.CreateFromAxisAngle(new xna.Vector3(0, 1, 0), (float)(Math.PI / 2)))));
_entity.InsertEntity(_irEntity);
// refresh the whole entity
System.Reflection.MethodInfo mInfo = typeof(simengine.SimulationEngine).GetMethod("RefreshEntity",
System.Reflection.BindingFlags.Instance | System.Reflection.BindingFlags.NonPublic | System.Reflection.BindingFlags.Public);
if (mInfo != null)
{
mInfo.Invoke(simengine.SimulationEngine.GlobalInstance, new object[] { _entity });
}
}
}
// start up the polling timer
int timerInterval = (_state.PollingInterval <= 0) ? 200 : _state.PollingInterval;
if ((timerInterval > 0 && timerInterval < 200))
{
timerInterval = 200;
}
StartTimer(timerInterval);
// create default state based on the physics entity
//_state.DistanceBetweenWheels = _entity.ChassisShape.BoxState.Dimensions.X;
//_state.LeftWheel.MotorState.PowerScalingFactor = _entity.MotorTorqueScaling;
//_state.RightWheel.MotorState.PowerScalingFactor = _entity.MotorTorqueScaling;
}
private IEnumerator <ITask> InitializeSpeechRecognizer()
{
// Register handlers for notification from speech recognizer
MainPortInterleave.CombineWith(new Interleave(
new ExclusiveReceiverGroup(
Arbiter.Receive <sr.SpeechDetected>(true, this.speechRecoNotifyPort, this.SpeechDetectedHandler),
Arbiter.Receive <sr.SpeechRecognized>(true, this.speechRecoNotifyPort, this.SpeechRecognizedHandler),
Arbiter.Receive <sr.SpeechRecognitionRejected>(true, this.speechRecoNotifyPort, this.SpeechRecognitionRejectedHandler),
Arbiter.Receive <sr.BeamDirectionChanged>(true, this.speechRecoNotifyPort, this.BeamDirectionChangedHandler)),
new ConcurrentReceiverGroup()));
// subscribe to speech recognizer notifications:
this.speechRecoPort.Subscribe(this.speechRecoNotifyPort);
// prepare the voice command dictionary with actions. It has to use semantics that are defined in MicArraySpeechRecognizer.user.config.xml
// the voice commands part can be left empty. You can have many items with the same semantics and handler:
speechRecognizerDictionary.Add(new SpeechRecognizerDictionaryItem("", "stop", VoiceCommandStopHandler));
speechRecognizerDictionary.Add(new SpeechRecognizerDictionaryItem("", "continue", VoiceCommandContinueHandler));
speechRecognizerDictionary.Add(new SpeechRecognizerDictionaryItem("", "turn left", VoiceCommandTurnLeftHandler));
speechRecognizerDictionary.Add(new SpeechRecognizerDictionaryItem("", "turn right", VoiceCommandTurnRightHandler));
speechRecognizerDictionary.Add(new SpeechRecognizerDictionaryItem("", "back up", VoiceCommandBackUpHandler));
speechRecognizerDictionary.Add(new SpeechRecognizerDictionaryItem("", "track humans", VoiceCommandTrackHumansHandler));
speechRecognizerDictionary.Add(new SpeechRecognizerDictionaryItem("", "here I am", VoiceCommandHereIAmHandler));
speechRecognizerDictionary.Add(new SpeechRecognizerDictionaryItem("", "shoot me", VoiceCommandShootMeHandler, 0.65f)); // higher confidence for shooting
speechRecognizerDictionary.Add(new SpeechRecognizerDictionaryItem("", "your master", VoiceCommandYourMasterHandler));
/*
*
* // You can set the dictionary only if it hasn't been set by existing C:\Microsoft Robotics Dev Studio 4\store\micarrayspeechrecognizer.user.config.xml
* // or by C:\Microsoft Robotics Dev Studio 4\projects\TrackRoamer\TrackRoamerServices\Config\MicArraySpeechRecognizer.user.config.xml and manifest
*
* // Wait 2 seconds:
* yield return Timeout(2000);
*
* // provide Grammar in the form of dictionary:
* sr.SpeechRecognizerState srState = new sr.SpeechRecognizerState();
* srState.GrammarType = sr.GrammarType.DictionaryStyle;
*
* srState.DictionaryGrammar = new DssDictionary<string, string>();
*
* for (int i = 0; i < speechRecognizerDictionary.Count; i++)
* {
* // make sure the voiceCommand part is not empty - contains actual pronounced command:
* srState.DictionaryGrammar.Add(speechRecognizerDictionary[i].voiceCommand.Trim(), speechRecognizerDictionary[i].semantics.Trim());
* }
*
* //srState.DictionaryGrammar.Add("stop", "stop");
*
* // Post replace request to SpeechRecognizer service and check outcome
* sr.Replace replaceRequest = new sr.Replace(srState);
* this.speechRecoPort.Post(replaceRequest);
*
* yield return (Choice)replaceRequest.ResponsePort;
* Fault fault = (Fault)replaceRequest.ResponsePort;
* if (fault != null)
* {
* string message = "Failed to set Dictionary - in Initialize Speech Recognizer";
* LogError(message);
* Talker.Say(10, message);
* yield break;
* }
*/
// Wait 2 seconds:
yield return(Timeout(2000));
Talker.Say(10, "talk to me");
}
/// <summary>
/// Allocation and assignments for first run
/// </summary>
/// <returns>True if initialization succeeded, otherwise False</returns>
private bool Initialize()
{
if (this.initialized)
{
return(this.initialized);
}
try
{
// No persisted state file, create a new one
if (this.state == null)
{
this.state = new TrackroamerRP2011AbstractionLayerState();
}
// Populate the IR sensor state
if (this.state.InfraredSensorState == null)
{
this.state.InfraredSensorState = new ir.InfraredSensorArrayState();
}
if (this.state.InfraredSensorState.Sensors == null)
{
this.state.InfraredSensorState.Sensors = new List <Microsoft.Robotics.Services.Infrared.InfraredState>();
}
if (this.state.InfraredSensorState.Sensors.Count == 0)
{
foreach (int irIdentifier in DefaultInfraredArrayIdentifiers)
{
this.state.InfraredSensorState.Sensors.Add(new Microsoft.Robotics.Services.Infrared.InfraredState()
{
HardwareIdentifier = irIdentifier
});
}
}
// Populate the Sonar sensor state
if (this.state.SonarSensorState == null)
{
this.state.SonarSensorState = new sonar.SonarSensorArrayState();
}
if (this.state.SonarSensorState.Sensors == null)
{
this.state.SonarSensorState.Sensors = new List <Microsoft.Robotics.Services.Sonar.SonarState>();
}
if (this.state.SonarSensorState.Sensors.Count == 0)
{
foreach (int sonarIdentifier in DefaultSonarArrayIdentifiers)
{
this.state.SonarSensorState.Sensors.Add(new Microsoft.Robotics.Services.Sonar.SonarState()
{
HardwareIdentifier = sonarIdentifier
});
}
}
if (this.state.BatteryState == null)
{
this.state.BatteryState = new Microsoft.Robotics.Services.Battery.BatteryState();
}
if (this.state.BatteryState.MaxBatteryPower == 0)
{
this.state.BatteryState.MaxBatteryPower = DefaultMaxBatteryPower;
}
}
catch (Exception e)
{
LogError(e);
this.Shutdown();
return(false);
}
this.state.LastStartTime = DateTime.Now;
SaveState(this.state);
base.Start();
// Make sure the pin polling port is in the main interleave because it modifies service state
MainPortInterleave.CombineWith(
new Interleave(
new ExclusiveReceiverGroup(
Arbiter.ReceiveWithIterator(true, this.sensorPollingPort, this.PollSensors),
Arbiter.Receive <proxibrick.UpdateSonarData>(true, _trackRoamerBrickProximityBoardServiceNotify, trpbUpdateSonarNotification),
Arbiter.Receive <proxibrick.UpdateProximityData>(true, _trackRoamerBrickProximityBoardServiceNotify, trpbUpdateProximityNotification),
Arbiter.Receive <proxibrick.UpdateParkingSensorData>(true, _trackRoamerBrickProximityBoardServiceNotify, trpbUpdateParkingSensorNotification)
)
, new ConcurrentReceiverGroup(
//Arbiter.Receive<drive.Update>(true, this.driveNotifyPort, this.DriveNotification)
)
));
//this.controllerDrivePort.Post(new drive.Subscribe() { NotificationPort = this.driveNotifyPort });
//this.controllerDrivePort.Post(new drive.ReliableSubscribe() { NotificationPort = this.driveNotifyPort });
Type[] notifyMeOf = new Type[] { typeof(proxibrick.UpdateSonarData), typeof(proxibrick.UpdateProximityData), typeof(proxibrick.UpdateParkingSensorData) };
_trackRoamerBrickProximityBoardServicePort.Subscribe(_trackRoamerBrickProximityBoardServiceNotify, notifyMeOf);
// Start the sensors polling interval
this.sensorPollingPort.Post(DateTime.Now);
return(true);
}
