private void Say(string word)
{
using (var tts = _connection.GetProxy <TextToSpeechProxy>())
{
tts.say(word);
}
}
/// <summary>
/// Disposes current object
/// </summary>
public void Dispose()
{
if (_worker.IsBusy)
{
_worker.CancelAsync();
}
_worker.Dispose();
try
{
// make sure we unsubribe to speech recognition proxy
using (var speechRecognitionProxy = _connection.GetProxy <SpeechRecognitionProxy>())
{
speechRecognitionProxy.unsubscribe(SUBSCRIBER_NAME);
}
}
catch {}
_connection = null;
}
/// <summary>
/// Do work event handler.
/// Turns until the robot sees a mark.
/// </summary>
/// <param name="sender"></param>
/// <param name="e"></param>
void _findMarkWorker_DoWork(object sender, DoWorkEventArgs e)
{
// reset the actual rotation
var actualRotation = 0f;
//init robot
InitRobotPosition();
// create a LandMarkDetectionProxy to start looking for marks
using (var landMarkDetectionProxy = _connection.GetProxy <LandMarkDetectionProxy>())
{
landMarkDetectionProxy.subscribe(SUBSCRIBER_NAME);
// create a MemoryProxy to read landmark information
using (var memProxy = _connection.GetProxy <MemoryProxy>())
{
// use one motion proxy
using (var motionProxy = _connection.GetProxy <MotionProxy>())
{
do
{// turn the robot 360 degrees
// reset the yaw to 0
MotionHelper.MoveHead(motionProxy, null, 0f, true);
// initialize the look direction
var startLookDirection = this.StartLookDirection;
var currentLookDirection = this.StartLookDirection;
do
{// make the robot look left and right
// reset head pitch position to 0
MotionHelper.MoveHead(motionProxy, 0f, null, true);
do
{ // make the robot look down
var landMarks = this.TryGetLandMark(memProxy);
if (landMarks != null && landMarks.ContainsKey(MarkID))
{
// align the robot to the landmark
AlignWithMark(motionProxy, landMarks[MarkID]);
// reload the mark information from memory because the robot moved
landMarks = this.TryGetLandMark(memProxy);
// should not be empty but just make sure
if (landMarks != null && landMarks.ContainsKey(MarkID))
{
// save the result
e.Result = new KeyValuePair <int, ArrayList>(MarkID, landMarks[MarkID]);
}
}
}while (e.Result == null && !_findMarkWorker.CancellationPending && LookDown(motionProxy));
}while (e.Result == null && !_findMarkWorker.CancellationPending &&
LookAround(motionProxy, startLookDirection, ref currentLookDirection));
}while (e.Result == null && !_findMarkWorker.CancellationPending && Rotate(motionProxy, ref actualRotation));
}
}
landMarkDetectionProxy.unsubscribe(SUBSCRIBER_NAME);
}
}
/// <summary>
/// Do work event handler.
/// Turns until the robot sees a mark.
/// </summary>
/// <param name="sender"></param>
/// <param name="e"></param>
void _findMarkWorker_DoWork(object sender, DoWorkEventArgs e)
{
// reset the actual rotation
var actualRotation = 0f;
//init robot
InitRobotPosition();
// create a LandMarkDetectionProxy to start looking for marks
using (var landMarkDetectionProxy = _connection.GetProxy <LandMarkDetectionProxy>())
{
landMarkDetectionProxy.subscribe(SUBSCRIBER_NAME, 100, -1f);
// create a MemoryProxy to read landmark information
using (var memProxy = _connection.GetProxy <MemoryProxy>())
{
// use one motion proxy
using (var motionProxy = _connection.GetProxy <MotionProxy>())
{
do
{// turn the robot 360 degrees
//// check for marks in a new thread while turning the robot's head
//Thread checkForMarks = new Thread(delegate()
// {
// Thread.Sleep(500);
// });
var methodID = LookForMarks(motionProxy);
while (motionProxy.isRunning(methodID))
{
// get the LandmarkDetected memory
var landMarkMemory = memProxy.getData("LandmarkDetected") as ArrayList;
if (_findMarkWorker.CancellationPending ||
(!landMarkMemory.IsNullOrEmpty() && (int)((ArrayList)((ArrayList)((ArrayList)((ArrayList)landMarkMemory)[1])[0])[1])[0] == MarkID))
{
motionProxy.stop(methodID);
if (!_findMarkWorker.CancellationPending)
{
// save the result
e.Result = new KeyValuePair <int, ArrayList>(MarkID, ((ArrayList)((ArrayList)((ArrayList)landMarkMemory)[1])[0]));
}
/*
* if (!_findMarkWorker.CancellationPending)
* {
* // align the robot to the landmark
* AlignWithMark(motionProxy, ((ArrayList)((ArrayList)((ArrayList)landMarkMemory)[1])[0]));
*
* // reload the mark information from memory because the robot moved
* var landMarks = this.TryGetLandMark(memProxy);
* // should not be empty but just make sure
* if (landMarks != null && landMarks.ContainsKey(MarkID))
* {
*
* }
* }
*/
}
/*
* // reload the mark information from memory because the robot moved
* var landMarks = this.TryGetLandMark(memProxy);
* // should not be empty but just make sure
* if (landMarks != null && landMarks.ContainsKey(MarkID))
* {
* motionProxy.stop(methodID);
* }*/
}
}while (e.Result == null && !_findMarkWorker.CancellationPending && Rotate(motionProxy, ref actualRotation));
}
}
landMarkDetectionProxy.unsubscribe(SUBSCRIBER_NAME);
}
}
/// <summary>
/// Walk do work event handler
/// </summary>
/// <param name="sender"></param>
/// <param name="e"></param>
void _walkWorker_DoWork(object sender, DoWorkEventArgs e)
{
// set the initial head yaw step and head pitch step amount to default but update it as it gets near to the mark
var headYawStepAmount = FindNaoMark.DEFAULT_HEAD_YAW_STEP_AMOUNT;
var headPitchStepAmount = FindNaoMark.DEFAULT_HEAD_PITCH_STEP_AMOUNT;
while (!_walkWorker.CancellationPending)
{
// clear previous detected mark
_detectedMark = null;
// find mark
FindNaoMark findMark = new FindNaoMark(this.MarkInfo.MarkID, this._robotConnectionHelper, headPitchStepAmount: headPitchStepAmount, headYawStepAmount: headYawStepAmount, startLookDirection: this.StartLookDirection);
findMark.NaoMarkDetected += new EventHandler <FindMarkEventArgs>(findMark_NaoMarkDetected);
findMark.StartSearching();
// wait until mark is detected
while (!_walkWorker.CancellationPending)
{
lock (_lockObject)
{
if (_detectedMark != null)
{
break;
}
}
Thread.Sleep(500);
}
// stop searching for mark
findMark.StopSearching();
findMark.Dispose();
if (!_walkWorker.CancellationPending)
{
// get camera transform
Transform cameraTransform = null;
using (var motionProxy = _robotConnectionHelper.GetProxy <MotionProxy>())
{
cameraTransform = new Transform(motionProxy.getTransform("CameraTop", 2, true) as List <float>);
// get mark position
var markPosition = LandMarkHelper.Instance.GetLandMarkPosition(_detectedMark, this.MarkInfo.MarkSize, cameraTransform);
// align with mark
AlignWithMark(motionProxy, _detectedMark);
// stop if close to mark
if (markPosition.x - 0.01 <= this.MarkInfo.StopDistance)
{
break;
}
else
{
if (markPosition.x <= 0.4)
{
headPitchStepAmount = FindNaoMark.DEFAULT_HEAD_PITCH_STEP_AMOUNT / 4;
headYawStepAmount = FindNaoMark.DEFAULT_HEAD_YAW_STEP_AMOUNT / 4;
}
else if (markPosition.x <= 0.6)
{
headPitchStepAmount = FindNaoMark.DEFAULT_HEAD_PITCH_STEP_AMOUNT / 2;
headYawStepAmount = FindNaoMark.DEFAULT_HEAD_YAW_STEP_AMOUNT / 2;
}
else if (markPosition.x <= 0.8)
{
headPitchStepAmount = 3 * FindNaoMark.DEFAULT_HEAD_PITCH_STEP_AMOUNT / 4;
headYawStepAmount = 3 * FindNaoMark.DEFAULT_HEAD_YAW_STEP_AMOUNT / 4;
}
// advance on x
var advance = markPosition.x - this.MarkInfo.AdvanceDistance <= this.MarkInfo.StopDistance ? markPosition.x - this.MarkInfo.StopDistance : this.MarkInfo.AdvanceDistance;
motionProxy.moveTo(advance, 0f, 0f);
}
}
}
}
}
