// Starts the specified servo move using the specified speed. StartSpeedMove is used by PersonTracking
// and RobotSpeech, which are lower priority moves than those issued through Sequences. StartSpeedMove
// motion can be interrupted by Sequence moves, and new StartSpeedMove moves are ignored if a Sequence
// is already moving this servo.
public void StartSpeedMove(ServoPosition servoPosition, long servoSpeed)
{
if (!IsEnabled())
{
ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Debug, "StartSpeedMove failed, servos are disabled.");
return;
}
ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Debug, "StartSpeedMove moving servo " + servoPosition.index.ToString() + " to position " + servoPosition.position.ToString() + " at speed " + servoSpeed.ToString() + ".");
Servo servo = servoList.Find(x => x.index == servoPosition.index);
lock (servo.movePriorityLock)
{
if (servo.isRunningHighPriorityMove)
{
// Ignore this command if a higher priority (i.e. Sequence) move is already running.
ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Debug, "StartSpeedMove, isRunningHighPriorityMove is set for servo " + servo.index.ToString() + ". Aborting move.");
return;
}
if (!IsConnected())
{
return;
}
SetServoSpeed(servo, servoSpeed);
SetServoPosition(servo, servoPosition.position);
}
}
// Sets the target position of the specified servo, causing this servo to begin moving
// to the new position. This target position is first bounded within the servo's min/max
// position limits, and a warning is logged if a position outside of these limits was
// specified.
private void SetServoPosition(Servo servo, double position)
{
if (position < servo.positionLimitMin)
{
ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Warning, "Requested servo " + servo.index.ToString() + " position " + position.ToString() + " bound to minimum limit " + servo.positionLimitMin.ToString());
// Bound to this limit.
position = servo.positionLimitMin;
}
if (position > servo.positionLimitMax)
{
ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Warning, "Requested servo " + servo.index.ToString() + " position " + position.ToString() + " bound to maximum limit " + servo.positionLimitMax.ToString());
// Bound to this limit.
position = servo.positionLimitMax;
}
ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Debug, "Setting servo " + servo.index.ToString() + " position to " + position.ToString());
try
{
// Send this value to the hardware.
// Note that the servo position values are handled in this class in units of μs,
// to match the convention used by Pololu's Maestro Control Center application.
// However, the servo controller hardware expects the position represented as an
// integer value in 0.25 μs. The local value must be multiplied by 4 to convert
// to these units.
uscDevice.setTarget((byte)servo.index, (ushort)(position * 4));
}
catch (System.Exception ex)
{
ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Error, "Caught exception in SetServoPosition(): " + ex.Message);
}
}
// Disables all servos by sending target position values of '0' to the hardware, for
// each servo. This '0' value has a special function of relaxing the servo, rather
// than moving to this position.
public void DisableAllServos()
{
ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Info, "Disabling all servos.");
lock (uscLock)
{
isEnabled = false;
}
foreach (Servo servo in servoList)
{
try
{
lock (uscLock)
{
if (uscDevice == null)
{
throw new System.Exception("uscDevice is null");
}
uscDevice.setTarget((byte)servo.index, 0);
}
}
catch (System.Exception ex)
{
ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Error, "Caught exception in DisableAllServos(): " + ex.Message);
}
}
}
// Disconnects from the Pololu Maestro servo controller.
// Based on the 'TryToDisconnect' method from MaestroAdvancedExample in the pololu-usb-sdk.
public void DisconnectFromHardware()
{
lock (uscLock)
{
if (uscDevice == null)
{
// Already disconnected
return;
}
try
{
uscDevice.Dispose(); // Disconnect
}
catch (Exception ex)
{
ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Error, "DisconnectFromHardware failed to cleaning disconnect the servo hardware: " + ex.Message);
}
finally
{
// do this no matter what
uscDevice = null;
}
}
}
// Moves both of the wheels to achieve the specified translation (in millimeters) and rotation (in degrees)
// over the time specified by timeToDestination (in seconds).
public void MoveBothWheels(double translationChange, double rotationChange, double timeToDestination)
{
if (!IsEnabled())
{
ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Warning, "MoveBothWheels failed, motion is disabled.");
return;
}
lock (movePriorityLock)
{
isRunningHighPriorityMove = true;
}
double translationVelocity = translationChange / timeToDestination;
double rotationVelocity = rotationChange / timeToDestination;
// Robot base is backwards with respect to torso, so translation velocity must be reversed.
translationVelocity = -translationVelocity;
StopTimer();
// Start wheel motion.
SetVelocity(translationVelocity);
SetRotationVelocity(rotationVelocity);
// Wait for motion to complete.
StartTimer(timeToDestination);
waitHandle.WaitOne();
lock (movePriorityLock)
{
isRunningHighPriorityMove = false;
}
}
// Sets the speed of the specified servo, in the servo controller hardware. This speed
// value is first bounded within the servo's min/max speed limits, and a warning is
// logged if a speed outside of these limits was specified.
private void SetServoSpeed(Servo servo, long speed)
{
if (speed < servo.speedLimitMin)
{
ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Warning, "Requested servo " + servo.index.ToString() + " speed " + speed.ToString() + " bound to minimum limit " + servo.speedLimitMin.ToString());
// Bound to this limit.
speed = servo.speedLimitMin;
}
if (speed > servo.speedLimitMax)
{
ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Warning, "Requested servo " + servo.index.ToString() + " speed " + speed.ToString() + " bound to maximum limit " + servo.speedLimitMax.ToString());
// Bound to this limit.
speed = servo.speedLimitMax;
}
ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Debug, "Setting servo " + servo.index.ToString() + " speed to " + speed.ToString());
try
{
// Send this value to the hardware.
uscDevice.setSpeed((byte)servo.index, (ushort)speed);
}
catch (System.Exception ex)
{
ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Error, "Caught exception in SetServoSpeed(): " + ex.Message);
}
}
// Moves the wheels to achieve the specified change in left and right wheel positions (in millimeters)
// over the time specified by timeToDestination (in seconds).
public void MoveEachWheel(double leftWheelChange, double rightWheelChange, double timeToDestination)
{
if (!IsEnabled())
{
ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Warning, "MoveEachWheel failed, motion is disabled.");
return;
}
lock (movePriorityLock)
{
isRunningHighPriorityMove = true;
}
double leftWheelVelocity = leftWheelChange / timeToDestination;
double rightWheelVelocity = rightWheelChange / timeToDestination;
// Robot base is backwards with respect to torso, so wheels are swapped (right is left) and velocity is reversed.
double newLeftWheelVelocity = -rightWheelVelocity;
double newRightWheelVelocity = -leftWheelVelocity;
StopTimer();
// Start wheel motion.
SetWheelVelocity(newLeftWheelVelocity, newRightWheelVelocity);
// Wait for motion to complete.
StartTimer(timeToDestination);
waitHandle.WaitOne();
lock (movePriorityLock)
{
isRunningHighPriorityMove = false;
}
}
// Connects to the Pololu Maestro servo controller through USB. Only one controller is
// currently expected, so this method just connects to the first controller it sees.
// Based on the 'connectToDevice' method from MaestroEasyExample in the pololu-usb-sdk.
public void ConnectToHardware()
{
try
{
DisconnectFromHardware();
// Get a list of all connected devices of this type.
List <DeviceListItem> connectedDevices = Usc.getConnectedDevices();
foreach (DeviceListItem dli in connectedDevices)
{
// If you have multiple devices connected and want to select a particular
// device by serial number, you could simply add a line like this:
// if (dli.serialNumber != "00012345"){ continue; }
uscDevice = new Usc(dli); // Connect to the device.
break; // Use first device (should only be one, anyway).
}
if (uscDevice == null)
{
ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Error, "ConnectToHardware() failed, no servo hardware found.");
}
else
{
ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Info, "ConnectToHardware() succeeded, connected to servo hardware.");
}
InitializeHardware();
}
catch (System.Exception ex)
{
ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Error, "Caught exception in Initialize(): " + ex.Message);
}
}
// Starts running the selected sequence in a new thread. Fails if another sequence
// is already running.
public void RunSequence(string sequenceName)
{
Sequence sequence = sequenceList.GetSequences().Find(x => x.name == sequenceName);
if (sequence == null)
{
ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Error, "Sequence '" + sequenceName + "' not found.");
return;
}
lock (sequenceLock)
{
if (sequenceIsRunning)
{
ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Warning, "RunSequence() failed to run sequence '" + sequenceName + "', another sequence is already running.");
return;
}
else
{
sequenceIsRunning = true;
}
}
runningSequence = sequence;
Thread newThread = new Thread(new ThreadStart(RunSequenceThread));
// Start the thread
newThread.Start();
}
// Starts performing all moves specified by servoPositionList, and then waits for these
// servos to finish moving.
public void MoveServos(List <ServoPosition> servoPositionList, double timeToDestination)
{
if (!IsEnabled())
{
ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Warning, "MoveServos failed, servos are disabled.");
return;
}
ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Debug, "MoveServos moving servos over " + timeToDestination + " seconds.");
foreach (ServoPosition servoPosition in servoPositionList)
{
Servo servo = servoList.Find(x => x.index == servoPosition.index);
lock (servo.movePriorityLock)
{
ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Debug, "MoveServos set isRunningHighPriorityMove for servo " + servo.index.ToString() + ".");
servo.isRunningHighPriorityMove = true;
}
}
if (!IsConnected())
{
// Simulate the move.
Thread.Sleep((int)(timeToDestination * 1000));
foreach (ServoPosition servoPosition in servoPositionList)
{
Servo servo = servoList.Find(x => x.index == servoPosition.index);
lock (servo.movePriorityLock)
{
ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Debug, "MoveServos cleared isRunningHighPriorityMove for servo " + servo.index.ToString() + ".");
servo.isRunningHighPriorityMove = false;
}
}
return;
}
foreach (ServoPosition servoPosition in servoPositionList)
{
StartTimedMove(servoPosition, timeToDestination);
}
WaitForMoveComplete(servoPositionList, timeToDestination);
foreach (ServoPosition servoPosition in servoPositionList)
{
Servo servo = servoList.Find(x => x.index == servoPosition.index);
lock (servo.movePriorityLock)
{
ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Debug, "MoveServos cleared isRunningHighPriorityMove for servo " + servo.index.ToString() + ".");
servo.isRunningHighPriorityMove = false;
}
}
}
// Polls the servo hardware to determine if the servos are still moving. Returns when all
// servos in servoPositionList have completed their moves.
private void WaitForMoveComplete(List <ServoPosition> servoPositionList, double timeToDestination)
{
// Create a list of servos to monitor.
List <Servo> servosToMonitor = new List <Servo>();
foreach (ServoPosition servoPosition in servoPositionList)
{
Servo servo = servoList.Find(x => x.index == servoPosition.index);
if (servo == null)
{
ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Error, "WaitForMoveComplete failed, servo " + servoPosition.index.ToString() + " not found.");
return;
}
servosToMonitor.Add(servo);
}
// Poll servo positions and wait until all servos reach their destinations.
double pollTimeout = timeToDestination + pollTimeoutAdjustment;
int pollTimeoutCount = (int)(pollTimeout * 1000 / (double)pollPeriod_ms);
int currentPollCount = 0;
while (true)
{
if (currentPollCount >= pollTimeoutCount)
{
ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Warning, "WaitForMoveComplete timeout, servos failed to reach destination in " + pollTimeout.ToString() + " seconds.");
return;
}
currentPollCount++;
UpdateServoValues();
// Determine if any servos in the list are still moving.
bool servoIsMoving = false;
foreach (Servo servo in servosToMonitor)
{
if (servo.isMoving)
{
servoIsMoving = true;
}
}
if (!servoIsMoving)
{
ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Debug, "WaitForMoveComplete succeeded, all servos reached destinations.");
return;
}
Thread.Sleep(pollPeriod_ms);
}
}
// Performs the sequence by stepping through each frame and executing the required
// voice synthesizer, delay, and motion actions.
private void RunSequenceThread()
{
if (!servoManager.IsConnected())
{
ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Warning, "Servo hardware is disconnected, running sequence '" + runningSequence.name + "' in simulation mode.");
}
// Disable the person tracking feature while a sequence is executing.
servoManager.PersonTrackingEnable(false);
ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Info, "RunSequence(): sequence '" + runningSequence.name + "' started.");
foreach (Frame frame in runningSequence.GetFrames())
{
// Run the speech synthesizer asynchronously. Speaks while continuing to
// perform moves in this and subsequent frames.
if (frame.speechString != null)
{
speechSynthesizer.SpeakAsync(frame.speechString);
}
// Go to the relevant PPT Slide
// perform moves in this and subsequent frames.
if (frame.pptIndex != null)
{
if (pptController.IsPptActive())
{
pptController.GoToSlide(frame.pptIndex);
}
}
// Wait for the specified amount of time.
if (frame.delay > 0)
{
Thread.Sleep((int)(frame.delay * 1000));
}
// Move all servos in the frame list.
if (frame.GetServoPositions().Count > 0)
{
servoManager.MoveServos(frame.GetServoPositions(), frame.timeToDestination);
}
}
ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Info, "RunSequence(): sequence '" + runningSequence.name + "' completed.");
// Re-enable the person tracking feature.
servoManager.PersonTrackingEnable(true);
lock (sequenceLock)
{
sequenceIsRunning = false;
}
}
public long GetServoIndex(string servoName)
{
Servo servo = servoList.Find(x => x.name == servoName);
if (servo != null)
{
return(servo.index);
}
else
{
ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Error, "GetServoIndex() failed to find servo '" + servoName + "'.");
return(-1);
}
}
// Sets the acceleration of the specified servo, in the servo controller hardware.
private void SetServoAcceleration(Servo servo, long acceleration)
{
ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Debug, "Setting servo " + servo.index.ToString() + " acceleration to " + acceleration.ToString());
try
{
// Send this value to the hardware.
uscDevice.setAcceleration((byte)servo.index, (ushort)acceleration);
}
catch (System.Exception ex)
{
ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Error, "Caught exception in SetServoAcceleration(): " + ex.Message);
}
}
// Stops the current motion of the wheels.
private void StopMotion()
{
ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Debug, "Stopping wheel motion.");
if (IsConnected())
{
lock (ariaLock)
{
// Always stop wheels, regardless of isEnabled flag.
setVel_dll(0);
setRotVel_dll(0);
}
}
}
public void SetEyeState(bool leftEyeOpen, bool rightEyeOpen)
{
ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Info, "SetEyeState setting left eye " + (leftEyeOpen ? "Open" : "Closed") + " and right eye " + (rightEyeOpen ? "Open" : "Closed") + ".");
lock (eyeStateLock)
{
leftEyeOpenState = leftEyeOpen;
rightEyeOpenState = rightEyeOpen;
if (!blinkInProgress)
{
SetHardwareState(leftEyeOpen, rightEyeOpen);
}
}
}
// Disables all servos by sending target position values of '0' to the hardware, for
// each servo. This '0' value has a special function of relaxing the servo, rather
// than moving to this position.
public void DisableAllServos()
{
ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Info, "Disabling all servos.");
foreach (Servo servo in servoList)
{
try
{
uscDevice.setTarget((byte)servo.index, 0);
}
catch (System.Exception ex)
{
ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Error, "Caught exception in DisableAllServos(): " + ex.Message);
}
}
}
// Calculates the required servo speed to reach the position specified by servoPosition,
// in the amount of time indicated by timeToDestination. Then starts this move and returns.
private void StartTimedMove(ServoPosition servoPosition, double timeToDestination)
{
Servo servo = servoList.Find(x => x.index == servoPosition.index);
ushort servoSpeed = 0;
if (servo == null)
{
ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Error, "StartTimedMove failed, servo " + servoPosition.index.ToString() + " not found.");
return;
}
if (servo.polledPosition == 0)
{
// If the servo position is 0, then the servo is off and we have no information about its actual position.
// We don't know the actual move distance, so just use the default servo speed.
servoSpeed = (ushort)servo.defaultSpeed;
}
else
{
// Convert current position to μs (the hardware uses 0.25 μs increments).
double currentPosition = ((double)servo.polledPosition) / 4;
// Position difference in μs.
double positionDifference = Math.Abs(servoPosition.position - currentPosition);
// Required speed in (μs/second).
double calculatedSpeed;
if (timeToDestination != 0)
{
calculatedSpeed = positionDifference / timeToDestination;
}
else
{
// If the desired move time is instantaneous, use the max allowed servo speed.
calculatedSpeed = servo.speedLimitMax;
}
// Convert speed from (1 μs / second) to (0.25 μs / 10 ms), used by the hardware.
servoSpeed = (ushort)(calculatedSpeed * (4.0 / 100.0));
}
SetServoSpeed(servo, servoSpeed);
SetServoPosition(servo, servoPosition.position);
}
// Sets the rotation velocity (deg/s) using both wheels.
private void SetRotationVelocity(double velocity)
{
ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Debug, "Setting wheel rotation velocity to " + velocity.ToString());
if (IsConnected())
{
lock (ariaLock)
{
if (!isEnabled)
{
return;
}
//ArRobot::setRotVel(velocity);
setRotVel_dll(velocity);
}
}
}
// Sets the velocity (mm/s) of each wheel separately.
private void SetWheelVelocity(double leftWheelVelocity, double rightWheelVelocity)
{
ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Debug, "Setting left wheel velocity to " + leftWheelVelocity.ToString() + " and right wheel velocity to " + rightWheelVelocity.ToString());
if (IsConnected())
{
lock (ariaLock)
{
if (!isEnabled)
{
return;
}
//ArRobot::setVel2(leftWheelVelocity, rightWheelVelocity);
setVel2_dll(leftWheelVelocity, rightWheelVelocity);
}
}
}
// Used by PersonTracking to track a person by rotating the robot. This tracking motion can be
// interrupted by moves issued through Sequences, since these are higher priority.
public void SetPersonTrackingRotation(double velocity)
{
if (!IsEnabled())
{
ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Debug, "SetPersonTrackingRotation failed, motion is disabled.");
return;
}
lock (movePriorityLock)
{
if (isRunningHighPriorityMove)
{
// Ignore this command if a higher priority (i.e. Sequence) move is already running.
return;
}
SetRotationVelocity(velocity);
}
}
// Loads the configuration settings for each servo from the xml file specified by 'fileName',
// and then initializes the servo hardware with these new settings. The InitializeHardware
// step can be skipped, if desired (e.g. on startup when the hardware is not yet connected).
public void LoadServoConfiguration(string fileName, bool initializeHardware = true)
{
ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Info, "Loading servo config from " + fileName);
XmlSerializer SerializerObj = new XmlSerializer(typeof(List <Servo>));
FileStream ReadFileStream = new FileStream(fileName, FileMode.Open, FileAccess.Read, FileShare.Read);
servoList = (List <Servo>)SerializerObj.Deserialize(ReadFileStream);
ReadFileStream.Close();
foreach (Servo servo in servoList)
{
ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Debug, "Loaded Servo " + servo.index + ": Name = " + servo.name + ", PosMin = " + servo.positionLimitMin + ", PosMax = " + servo.positionLimitMax + ", SpeedMin = " + servo.speedLimitMin + ", SpeedMax = " + servo.speedLimitMax);
}
if (initializeHardware)
{
InitializeHardware();
}
}
public void InitializeAriaHardware()
{
// Connect to Aria hardware
int hr = InitializeRobot_dll();
switch (hr)
{
case 0:
ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Info, "InitializeAria() succeeded, connected to robot base control.");
break;
case 1:
ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Error, "InitializeAria() failed, could not parse arguments.");
break;
case 2:
ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Error, "InitializeAria() failed, could not connect to robot base.");
break;
}
isHardwareInitialized = true;
}
// Sets the servo controller with the default speeds and accelerations, for each servo.
// This should be called whenever servo hardware is connected, or when new default
// servo parameters have been loaded.
private void InitializeHardware()
{
if (!IsConnected())
{
ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Error, "InitializeHardware() failed, not connected to servo hardware.");
return;
}
if (servoList.Count == 0)
{
ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Error, "InitializeHardware() failed, no servos have been defined.");
return;
}
foreach (Servo servo in servoList)
{
SetServoSpeed(servo, servo.defaultSpeed);
SetServoAcceleration(servo, servo.defaultAcceleration);
UpdateServoValues();
}
ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Info, "InitializeHardware() succeeded, servo hardware was initialized.");
}
// [Add new GestureTriggers here]
// Raised whenever new skeleton data arrives from the Kinect sensor.
// Updates person tracking and performs gesture recognition.
private void SensorSkeletonFrameReady(object sender, SkeletonFrameReadyEventArgs e)
{
Skeleton[] skeletons = new Skeleton[0];
using (SkeletonFrame skeletonFrame = e.OpenSkeletonFrame())
{
if (skeletonFrame != null)
{
skeletons = new Skeleton[skeletonFrame.SkeletonArrayLength];
skeletonFrame.CopySkeletonDataTo(skeletons);
}
}
if (skeletons.Length != 0)
{
bool isTrackingSameSkeleton = false;
int skeletonIdClosestToCenter = 0;
float shortestDistanceFromCenter = 0;
// First, check if this skeleton frame contains the skeleton that we were already tracking.
foreach (Skeleton skel in skeletons)
{
if (skel.TrackingState == SkeletonTrackingState.Tracked)
{
if (skel.TrackingId == trackedSkeletonId)
{
isTrackingSameSkeleton = true;
break;
}
else if (skeletonIdClosestToCenter == 0 || Math.Abs(skel.Joints[JointType.ShoulderCenter].Position.X) < shortestDistanceFromCenter)
{
skeletonIdClosestToCenter = skel.TrackingId;
shortestDistanceFromCenter = Math.Abs(skel.Joints[JointType.ShoulderCenter].Position.X);
}
}
}
// If the skeleton frame does not contain the one we were tracking, then track the skeleton that is closest to the center of the image (x-axis).
if (!isTrackingSameSkeleton)
{
trackedSkeletonId = skeletonIdClosestToCenter;
ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Debug, "SensorSkeletonFrameReady() now tracking skeleton ID " + trackedSkeletonId.ToString() + ".");
}
// Update person tracking and check gestures from the tracked skeleton.
foreach (Skeleton skel in skeletons)
{
// Only pay attention to the tracked skeleton.
if (skel.TrackingId == trackedSkeletonId)
{
// Only pay attention to the skeleton if it is within the right and left edges of the view.
// This should help prevent detecting false gestures, due to the skeleton being scrambled as it leaves the viewing area.
if ((skel.ClippedEdges & FrameEdges.Left) == 0 && (skel.ClippedEdges & FrameEdges.Right) == 0)
{
personTracking.UpdatePosition(skel.Joints[JointType.ShoulderCenter].Position);
if (gestureRecognitionEnabled)
{
// Check gesture recognition conditions.
//leftHandGesture.Evaluate(sequenceProcessor, skel.Joints[JointType.HandLeft].Position.Y > skel.Joints[JointType.ElbowLeft].Position.Y, "Left Hand Raised");
//rightHandGesture.Evaluate(sequenceProcessor, skel.Joints[JointType.HandRight].Position.Y > skel.Joints[JointType.ElbowRight].Position.Y, "Right Hand Raised");
RightHandAboveHead.Evaluate(sequenceProcessor, skel.Joints[JointType.HandRight].Position.Y > skel.Joints[JointType.Head].Position.Y, "Hello");
RightHandBetweenSpineAndShoulderCenter.Evaluate(sequenceProcessor, skel.Joints[JointType.HandRight].Position.Y > skel.Joints[JointType.Spine].Position.Y && skel.Joints[JointType.HandRight].Position.Y < skel.Joints[JointType.ShoulderCenter].Position.Y && skel.Joints[JointType.HandRight].Position.X < (skel.Joints[JointType.Spine].Position.X + 0.1), "PLAYMUSIC");
LeftElbowAboveLeftShoulder.Evaluate(sequenceProcessor, skel.Joints[JointType.ElbowLeft].Position.Y > skel.Joints[JointType.ShoulderLeft].Position.Y, "NeckRotate");
WalkCloseToKinect.Evaluate(sequenceProcessor, skel.Joints[JointType.Spine].Position.Z < 1.5, "ArmRaise");
// [Add new Evaluate calls here]
}
}
else
{
ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Debug, "SensorSkeletonFrameReady() skipped update due to skeleton being outside of view.");
}
break;
}
}
}
}
/// <summary>
/// Handler for recognized speech events.
/// </summary>
/// <param name="sender">object sending the event.</param>
/// <param name="e">event arguments.</param>
private void SpeechRecognized(object sender, SpeechRecognizedEventArgs e)
{
if (!speechRecognitionEnabled)
{
return;
}
// Check if the speech synthesizer is already speaking. Ignore any detected
// speech during this time, since it might have been generated by the synthesizer.
lock (speechLock)
{
if (synthesizerIsSpeaking)
{
ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Warning, "SpeechRecognized: Ignored word '" + e.Result.Text + "', since synthesizer is speaking.");
return;
}
}
ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Info, "SpeechRecognized: Detected word '" + e.Result.Text + "' with confidence " + e.Result.Confidence.ToString());
// Speech utterance confidence below which we treat speech as if it hadn't been heard
const double ConfidenceThreshold = 0.5;
if (e.Result.Confidence >= ConfidenceThreshold)
{
string sequenceName = "";
// Perform the specified Sequence depending on the recognized speech.
switch (e.Result.Semantics.Value.ToString())
{
case "Hello":
sequenceName = "Hello";
break;
case "HowAreUDoing":
sequenceName = "HowAreUDoing";
break;
case "Introduce":
sequenceName = "Introduce";
break;
case "Number":
sequenceName = "Number";
break;
case "DoWhat":
sequenceName = "DoWhat";
break;
case "Thanks":
sequenceName = "Thanks";
break;
case "Count":
sequenceName = "Count";
break;
case "OneAndTwo":
sequenceName = "OneAndTwo";
break;
case "FiveMinusOne":
sequenceName = "FiveMinusOne";
break;
case "Gesture":
sequenceName = "Gesture";
break;
case "NextOne":
sequenceName = "NextOne";
break;
case "NextTwo":
sequenceName = "NextTwo";
break;
case "NextThree":
sequenceName = "NextThree";
break;
case "PlayHarp":
sequenceName = "PlayHarp";
break;
case "PLAYMUSIC":
sequenceName = "PLAYMUSIC";
break;
case "lab":
sequenceName = "lab";
break;
case "GoAhead":
sequenceName = "GoAhead";
break;
case "NAME":
sequenceName = "NAME";
break;
case "Bye":
sequenceName = "Bye";
break;
case "GoodMorning":
sequenceName = "GoodMorning";
break;
case "URwelcome":
sequenceName = "URwelcome";
break;
case "GladSU":
sequenceName = "GladSU";
break;
case "DEFAULT":
sequenceName = "DEFAULT";
break;
}
if (sequenceName != "")
{
sequenceProcessor.RunSequence(sequenceName);
}
}
else
{
// If speech is not recognized, then use a random default response.
Random randomGenerator = new Random();
int randomSequenceNumber = randomGenerator.Next(1, 5);
string sequenceName = "DEFAULT" + randomSequenceNumber.ToString();
if (sequenceName != "")
{
sequenceProcessor.RunSequence(sequenceName);
}
}
}
// Reads the servo status from the servo controller hardware, and then updates the
// servoList with these new polled values. Also tracks whether each servo is currently
// moving by comparing their current and target positions.
public void UpdateServoValues()
{
if (!IsConnected())
{
ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Error, "UpdateServoValues() failed, not connected to servo hardware.");
return;
}
if (servoList.Count == 0)
{
ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Error, "UpdateServoValues() failed, no servos have been defined.");
return;
}
try
{
// Get the servo parameters from the hardware.
ServoStatus[] servoStatusArray;
uscDevice.getVariables(out servoStatusArray);
// Update the servoList with these parameters.
foreach (Servo servo in servoList)
{
if (servo.index < 0 || servo.index >= uscDevice.servoCount)
{
ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Error, "UpdateServoValues() failed, servo index out of range. Servo index = " + servo.index.ToString());
}
else
{
servo.polledPosition = servoStatusArray[servo.index].position;
servo.polledTarget = servoStatusArray[servo.index].target;
servo.polledSpeed = servoStatusArray[servo.index].speed;
servo.polledAcceleration = servoStatusArray[servo.index].acceleration;
ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Debug, "Servo " + servo.index.ToString() + ": Target = " + servo.polledTarget.ToString() + ", Position = " + servo.polledPosition.ToString() + ", Speed = " + servo.polledSpeed.ToString() + ", Acceleration = " + servo.polledAcceleration.ToString());
if (servo.isMoving == false && servo.polledTarget != servo.polledPosition)
{
// Servo has started moving.
ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Debug, "Servo " + servo.index + " has started moving from " + servo.polledPosition.ToString() + " to " + servo.polledTarget.ToString());
servo.isMoving = true;
}
else if (servo.isMoving == true && servo.polledTarget == servo.polledPosition)
{
// Servo has stopped moving.
ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Debug, "Servo " + servo.index + " has stopped moving at " + servo.polledPosition.ToString());
servo.isMoving = false;
}
if (servo.isMoving)
{
ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Debug, "Servo " + servo.index + " is at position " + servo.polledPosition.ToString());
}
}
}
}
catch (System.Exception ex)
{
ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Error, "Caught exception in UpdateServoValues(): " + ex.Message);
}
}
/// <summary>
/// Handler for recognized speech events.
/// </summary>
/// <param name="sender">object sending the event.</param>
/// <param name="e">event arguments.</param>
private void SpeechRecognized(object sender, SpeechRecognizedEventArgs e)
{
if (!speechRecognitionEnabled)
{
return;
}
// Check if the speech synthesizer is already speaking. Ignore any detected
// speech during this time, since it might have been generated by the synthesizer.
lock (speechLock)
{
if (synthesizerIsSpeaking)
{
ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Warning, "SpeechRecognized: Ignored word '" + e.Result.Text + "', since synthesizer is speaking.");
return;
}
}
ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Info, "SpeechRecognized: Detected word '" + e.Result.Text + "' with confidence " + e.Result.Confidence.ToString());
// Speech utterance confidence below which we treat speech as if it hadn't been heard
const double ConfidenceThreshold = 0.5;
if (e.Result.Confidence >= ConfidenceThreshold)
{
string sequenceName = "";
// Perform the specified Sequence depending on the recognized speech.
switch (e.Result.Semantics.Value.ToString())
{
case "Hello":
sequenceName = "Hello";
break;
case "PlayHarp":
sequenceName = "PlayHarp";
break;
case "PLAYMUSIC":
sequenceName = "PLAYMUSIC";
break;
case "Count":
sequenceName = "Count";
break;
case "NAME":
sequenceName = "NAME";
break;
case "DEFAULT":
sequenceName = "DEFAULT";
break;
}
if (sequenceName != "")
{
sequenceProcessor.RunSequence(sequenceName);
}
}
}
// Performs the sequence by stepping through each frame and executing the required
// voice synthesizer, delay, and motion actions.
private void RunSequenceThread()
{
if (!servoManager.IsConnected())
{
ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Warning, "Servo hardware is disconnected, running servos in simulation mode for sequence '" + runningSequence.name + "'.");
}
if (!ariaManager.IsConnected())
{
ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Warning, "Robot base hardware is disconnected, running robot base in simulation mode for sequence '" + runningSequence.name + "'.");
}
ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Info, "RunSequenceThread(): sequence '" + runningSequence.name + "' started.");
foreach (Frame frame in runningSequence.GetFrames())
{
// Run the speech synthesizer asynchronously. Speaks while continuing to
// perform moves in this and subsequent frames.
if (frame.speechString != null)
{
robotSpeech.Speak(frame.speechString);
}
// Set the state of the LED eyes.
if (frame.eyeState != null)
{
switch (frame.eyeState)
{
case "Open":
robotEyes.SetEyeState(true, true);
break;
case "Closed":
robotEyes.SetEyeState(false, false);
break;
case "LeftClosed":
robotEyes.SetEyeState(false, true);
break;
case "RightClosed":
robotEyes.SetEyeState(true, false);
break;
}
}
// Wait for the specified amount of time.
if (frame.delay > 0)
{
Thread.Sleep((int)(frame.delay * 1000));
}
// Servo motion and wheel motion are performed simultaneously by creating two separate threads and
// waiting until both threads have completed their motion.
ManualResetEvent[] manualEvents = new ManualResetEvent[]
{
new ManualResetEvent(false),
new ManualResetEvent(false)
};
// Start servo motion thread.
Thread servoThread = new Thread(new ParameterizedThreadStart(MoveServos));
servoThread.Start(new SequenceThreadData(frame, manualEvents[0]));
// Start wheel motion thread.
Thread wheelsThread = new Thread(new ParameterizedThreadStart(MoveWheels));
wheelsThread.Start(new SequenceThreadData(frame, manualEvents[1]));
// Wait until both servo motion and wheel motion has completed.
WaitHandle.WaitAll(manualEvents);
}
ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Info, "RunSequenceThread(): sequence '" + runningSequence.name + "' completed.");
lock (sequenceLock)
{
sequenceIsRunning = false;
}
}
// Connect to the Kinect sensor and begin processing events.
public void InitializeKinect()
{
this.sensor = null;
// Look through all sensors and start the first connected one.
// This requires that a Kinect is connected at the time of app startup.
// To make your app robust against plug/unplug,
// it is recommended to use KinectSensorChooser provided in Microsoft.Kinect.Toolkit (See components in Toolkit Browser).
foreach (var potentialSensor in KinectSensor.KinectSensors)
{
if (potentialSensor.Status == KinectStatus.Connected)
{
this.sensor = potentialSensor;
break; // Connect to first Kinect.
}
}
if (null == this.sensor)
{
ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Error, "InitializeKinect() failed, not connected to Kinect sensor.");
return;
}
else
{
// Turn on the skeleton stream to receive skeleton frames
this.sensor.SkeletonStream.Enable();
// Add an event handler to be called whenever there is new color frame data
SkeletonFrameReady += SensorSkeletonFrameReady;
// Start the sensor!
try
{
this.sensor.Start();
}
catch (IOException)
{
ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Error, "InitializeKinect() failed, unable to Start Kinect sensor.");
this.sensor = null;
return;
}
}
RecognizerInfo ri = GetKinectRecognizer();
if (null != ri)
{
this.speechEngine = new SpeechRecognitionEngine(ri.Id);
/****************************************************************
*
* Use this code to create grammar programmatically rather than from
* a grammar file.
*
* var directions = new Choices();
* directions.Add(new SemanticResultValue("forward", "FORWARD"));
* directions.Add(new SemanticResultValue("forwards", "FORWARD"));
* directions.Add(new SemanticResultValue("straight", "FORWARD"));
* directions.Add(new SemanticResultValue("backward", "BACKWARD"));
* directions.Add(new SemanticResultValue("backwards", "BACKWARD"));
* directions.Add(new SemanticResultValue("back", "BACKWARD"));
* directions.Add(new SemanticResultValue("turn left", "LEFT"));
* directions.Add(new SemanticResultValue("turn right", "RIGHT"));
*
* var gb = new GrammarBuilder { Culture = ri.Culture };
* gb.Append(directions);
*
* var g = new Grammar(gb);
*
****************************************************************/
// Create a grammar from grammar definition XML file.
using (var memoryStream = new MemoryStream(Encoding.ASCII.GetBytes(Properties.Resources.SpeechGrammar)))
{
var g = new Grammar(memoryStream);
speechEngine.LoadGrammar(g);
}
speechEngine.SpeechRecognized += SpeechRecognized;
speechEngine.SpeechRecognitionRejected += SpeechRejected;
robotSpeech.SpeakStarted += SpeakStarted;
robotSpeech.SpeakCompleted += SpeakCompleted;
// For long recognition sessions (a few hours or more), it may be beneficial to turn off adaptation of the acoustic model.
// This will prevent recognition accuracy from degrading over time.
////speechEngine.UpdateRecognizerSetting("AdaptationOn", 0);
speechEngine.SetInputToAudioStream(
sensor.AudioSource.Start(), new SpeechAudioFormatInfo(EncodingFormat.Pcm, 16000, 16, 1, 32000, 2, null));
speechEngine.RecognizeAsync(RecognizeMode.Multiple);
}
else
{
ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Error, "InitializeKinect() failed, no speech recognizer.");
}
ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Info, "InitializeKinect() succeeded, Kinect sensor is ready.");
}
