private void UpdatePolygon(RobotPose p)
{
Matrix3 transMat = Matrix3.Translation(p.x, p.y) * Matrix3.Rotation(p.yaw - Math.PI / 2.0);
bodyPlygn.Clear();
bodyPlygn.AddRange(transMat.TransformPoints(origPlygn));
}
/// <summary>
/// Finds gaps between polygon objects indiscriminately.
/// </summary>
/// <param name="polys">List of polygons to find gaps between</param>
/// <returns>List of LineSegment objects representing gaps found</returns>
private List <LineSegment> FindAllGaps(List <Polygon> polys, RobotPose pose)
{
List <LineSegment> gaps = new List <LineSegment>();
for (int i = 0; i < polys.Count; i++)
{
Polygon poly1 = polys[i];
for (int j = i + 1; j < polys.Count; j++)
{
Polygon poly2 = polys[j];
LineSegment gap = poly1.ShortestLineToOther(poly2);
if (gap.Length <= ignoreabovethis)
{
Vector2 startGlobal = new Vector2(Math.Cos(pose.yaw) * gap.P0.X - Math.Sin(pose.yaw) * gap.P0.Y + pose.x,
Math.Sin(pose.yaw) * gap.P0.X + Math.Cos(pose.yaw) * gap.P0.Y + pose.y);
Vector2 endGlobal = new Vector2(Math.Cos(pose.yaw) * gap.P1.X - Math.Sin(pose.yaw) * gap.P1.Y + pose.x,
Math.Sin(pose.yaw) * gap.P1.X + Math.Cos(pose.yaw) * gap.P1.Y + pose.y);
LineSegment gapGlobal = new LineSegment(startGlobal, endGlobal);
gaps.Add(gapGlobal);
}
}
}
return(gaps);
}
/// <summary>
/// Find targets above grounds and return in global and robot coordinate
/// </summary>
/// <param name="robotPose"></param>
/// <param name="laserScan"></param>
/// <param name="localPoints"></param>
/// <param name="globalPoints"></param>
public void DetectTarget(RobotPose robotPose, ILidarScan <ILidar2DPoint> laserScan, out List <Vector3> laserPoints, out List <Vector3> localPoints, bool lidarUpsideDown)
{
// initialization
localPoints = new List <Vector3>();
laserPoints = new List <Vector3>();
for (int i = 0; i < laserScan.Points.Count; i++)
{
Vector4 localPt = laserToRobot * laserScan.Points[i].RThetaPoint.ToVector4();
Vector4 newLocalPt;
if (lidarUpsideDown)
{
newLocalPt = new Vector4(-localPt.Y, localPt.X, localPt.Z, localPt.W);
}
else
{
newLocalPt = new Vector4(localPt.Y, localPt.X, localPt.Z, localPt.W);
}
localPoints.Add(new Vector3(newLocalPt.X, newLocalPt.Y, newLocalPt.Z));
if (lidarUpsideDown)
{
laserPoints.Add(new Vector3(laserScan.Points[i].RThetaPoint.R, laserScan.Points[i].RThetaPoint.theta, laserScan.Points[i].RThetaPoint.thetaDeg));
}
else
{
laserPoints.Add(new Vector3(laserScan.Points[laserScan.Points.Count - i - 1].RThetaPoint.R,
laserScan.Points[laserScan.Points.Count - i - 1].RThetaPoint.theta,
laserScan.Points[laserScan.Points.Count - i - 1].RThetaPoint.thetaDeg));
}
}
}
public Vector2 rotateTranslate(Vector2 X, RobotPose xParms)
{
Vector2 XT = new Vector2(X.X * Math.Cos(xParms.yaw) - X.Y * Math.Sin(xParms.yaw) + xParms.x,
X.X * Math.Sin(xParms.yaw) + X.Y * Math.Cos(xParms.yaw) + xParms.y);
return(XT);
}
public LidarPosePackageMessage(int robotID, RobotPose pose, SensorPose sensorPose, ILidarScan<ILidar2DPoint> scan)
{
this.robotID = robotID;
this.pose = pose;
this.lidarScan = scan;
this.sensorPose = sensorPose;
}
/// <summary>
/// Create a LocalMapRequestMessage that is sent from robots to the CentralSensorProcessor for a updated local map
/// </summary>
/// <param name="robotID">robotID</param>
/// <param name="currentPose">current position</param>
/// <param name="extentX">x-size of the local occupancy map</param>
/// <param name="extentY">y-size of the local occupancy map </param>
public LocalMapRequestMessage(int robotID, RobotPose currentPose, double extentX, double extentY)
{
this.robotID = robotID;
this.currentPose = currentPose;
this.extentX = extentX;
this.extentY = extentY;
}
public unsafe void SetCellsFast(Bitmap bmp, RobotPose pose, IOccupancyGrid2D og, int setDiameter)
{
BitmapData data = bmp.LockBits(new Rectangle(0, 0, bmp.Width, bmp.Height),
ImageLockMode.ReadOnly, PixelFormat.Format24bppRgb);
byte *imgPtr = (byte *)(data.Scan0);
byte red, green, blue;
int poseIdxX, poseIdxY;
og.GetIndicies(pose.x, pose.y, out poseIdxX, out poseIdxY);
imgPtr += ((poseIdxY - setDiameter / 2) * data.Stride) + (poseIdxX - setDiameter / 2) * 3;
for (int i = 0; i < setDiameter; i++)
{
for (int j = 0; j < setDiameter; j++)
{
blue = imgPtr[0];
green = imgPtr[1];
red = imgPtr[2];
occupancyMap[j, i] = (double)blue;
imgPtr += 3;
}
imgPtr += (data.Width - setDiameter) * 3;
}
bmp.UnlockBits(data);
}
public void UpdatePose(RobotPose pose)
{
lock (locker)
{
this.currentPose = pose;
}
}
public ViconPoseProvider(String ip, Int32 port, String subject, String segment)
{
this.ip = IPAddress.Parse(ip);
this.port = port;
this.subject = subject;
this.segment = segment;
wantedChannelNames = new List <string>(6);
pose = new RobotPose();
vicon = new ViconTarsus(ip, port);
vicon.Start();
vicon.DataReceived += new EventHandler <IViconFrameEventArgs>(vicon_DataReceived);
wantedChannelNames.Add(subject + ":" + segment + " <t-X>");
wantedChannelNames.Add(subject + ":" + segment + " <t-Y>");
wantedChannelNames.Add(subject + ":" + segment + " <t-Z>");
wantedChannelNames.Add(subject + ":" + segment + " <a-X>");
wantedChannelNames.Add(subject + ":" + segment + " <a-Y>");
wantedChannelNames.Add(subject + ":" + segment + " <a-Z>");
wantedChannelNumbers = vicon.GetChannelNumbers(wantedChannelNames);
vicon.SetStreamingChannels(wantedChannelNumbers);
vicon.SetStreaming(true);
}
static void poseClient_MsgReceived(object sender, MsgReceivedEventArgs <RobotPoseMessage> e)
{
if (e.message == null)
{
return;
}
pose = e.message.Pose;
protoPose.x = pose.x;
protoPose.y = pose.y;
protoPose.z = pose.z;
protoPose.yaw = pose.yaw;
protoPose.pitch = pose.pitch;
protoPose.roll = pose.roll;
protoPose.timeStamp = pose.TimeStamp;
protoPoseServer.SendUnreliably(protoPose);
// Clear the screen
Console.Clear();
Console.SetCursorPosition(Console.CursorLeft, Console.CursorTop);
Console.WriteLine("Robot pose:");
Console.WriteLine("X:\t" + pose.x);
Console.WriteLine("Y:\t" + pose.y);
Console.WriteLine("Z:\t" + pose.z);
Console.WriteLine("yaw:\t" + pose.yaw);
Console.WriteLine("pitch:\t" + pose.pitch);
Console.WriteLine("roll:\t" + pose.roll);
Console.WriteLine("timestamp:\t" + pose.timestamp);
}
public VFHFollower(SensorPose sickPose, SensorPose hokuyoPose, double angularResolution, bool useHokuyo)
{
pose = null;
this.useHokuyo = useHokuyo;
this.sickPose = sickPose;
sickDCM = Matrix4.FromPose(this.sickPose);
if (this.useHokuyo)
{
this.hokuyoPose = hokuyoPose;
hokuyoDCM = Matrix4.FromPose(this.hokuyoPose);
}
sickHistory = new List <Vector2[]>();
hokuyoHistory = new List <Vector2[]>();
this.angularResolution = angularResolution;
numBuckets = (int)Math.Round(360 / this.angularResolution);
trapped = false;
kThreshold = (int)Math.Round(160 / this.angularResolution);
rHistogram = new Double[numBuckets];
}
public void UpdateMeanCovariance(List <Vector2> pt, List <Matrix> cov, RobotPose currentPose)
{
mean.Clear(); covariance.Clear();
if (pt.Count != cov.Count)
{
throw new ArgumentException("Length of List<Vector2> and List<Matrix> do not match");
}
else
{
for (int i = 0; i < pt.Count; i++)
{
double x = pt[i].X; // pt[i].X * Math.Cos(currentPose.yaw) - pt[i].Y * Math.Sin(currentPose.yaw) + currentPose.x;
double y = pt[i].Y; // pt[i].X * Math.Sin(currentPose.yaw) + pt[i].Y * Math.Cos(currentPose.yaw) + currentPose.y;
mean.Add(new List <Vector2>());
covariance.Add(new List <Vector2>());
// mean
//mean[i].Add(new Vector2(x - width / 2, y - width / 2));
//mean[i].Add(new Vector2(x + width / 2, y - width / 2));
//mean[i].Add(new Vector2(x + width / 2, y + width / 2));
//mean[i].Add(new Vector2(x - width / 2, y + width / 2));
mean[i].Add(new Vector2(x, y));
// covariance ellipse
Matrix ellipse = GenerateEllipse(numCovPt, x, y, cov[i], sigma);
for (int j = 0; j < ellipse.Rows; j++)
{
covariance[i].Add(new Vector2(ellipse[j, 0], ellipse[j, 1]));
}
}
}
}
public LidarPoseTargetMessage(int robotID, int targetID, RobotPose detectionPose, ILidarScan <ILidar2DPoint> scan)
{
this.robotID = robotID;
this.targetID = targetID;
this.detectionPose = detectionPose;
this.scan = scan;
}
public LidarPosePackageMessage(int robotID, RobotPose pose, SensorPose sensorPose, ILidarScan <ILidar2DPoint> scan)
{
this.robotID = robotID;
this.pose = pose;
this.lidarScan = scan;
this.sensorPose = sensorPose;
}
public LidarPoseTargetMessage(int robotID, int targetID, RobotPose detectionPose, ILidarScan<ILidar2DPoint> scan)
{
this.robotID = robotID;
this.targetID = targetID;
this.detectionPose = detectionPose;
this.scan = scan;
}
public void UpdateGoal(RobotPose goal)
{
lock (followerLock)
{
this.goalPoint.X = goal.x;
this.goalPoint.Y = goal.y;
}
}
public RRTPlannerImpl(RobotTwoWheelStateProvider stateProvider)
{
rrt = new RRTPlannerControl();
obstacles = new List <Polygon>();
currentPose = new RobotPose();
waypoints = new PointPath();
this.stateProvider = stateProvider;
}
public void UpdatePose(RobotPose robotPose)
{
try
{
UpdatePose(robotPose.x, robotPose.y, robotPose.yaw);
}
catch (NullReferenceException e) { }
}
public void UpdatePose(RobotPose pose)
{
{
this.currentPoint.x = pose.x;
this.currentPoint.y = pose.y;
this.currentPoint.yaw = pose.yaw;
}
}
public TargetInformation(int targetID, RobotPose targetState, Matrix targetCov, RobotPose detectedPose, TargetTypes type)
{
this.targetID = targetID;
this.targetState = targetState;
this.targetCov = targetCov;
this.detectedPose = detectedPose;
this.type = type;
}
public void UpdatePose(RobotPose pose)
{
lock (followerLock)
{
this.currentPoint.x = pose.x;
this.currentPoint.y = pose.y;
this.currentPoint.yaw = pose.yaw;
}
}
public void UpdatePose(RobotPose pose)
{
if (pose == null)
{
return;
}
this.pose = pose;
}
/// <summary>
/// Return a 2D vector point in EN framework given two robot pose and pixel reading
/// </summary>
/// <param name="r1">Robot1 pose</param>
/// <param name="r2">Robot2 pose</param>
/// <param name="pixelR1">pixel reading from robot1</param>
/// <param name="pixelR2">pixel reading from robot2</param>
/// <param name="focalLength">focal length of the camera you're using</param>
/// <returns>2D point of target for initialization</returns>
public static Vector2 ComputeInitialPoint(RobotPose r1, RobotPose r2, Vector2 pixelR1, Vector2 pixelR2, double centerPixX, double focalLength)
{
double u1 = pixelR1.X; double u2 = pixelR2.X;
double gAngle1 = r1.yaw - Math.Atan2(u1 - centerPixX, focalLength);
double gAngle2 = r2.yaw - Math.Atan2(u2 - centerPixX, focalLength);
double xGoal = ((r2.y - r1.y) - Math.Tan(gAngle2) * r2.x + Math.Tan(gAngle1) * r1.x) / (Math.Tan(gAngle1) - Math.Tan(gAngle2));
double yGoal = Math.Tan(gAngle1) * (xGoal - r1.x) + r1.y;
return(new Vector2(xGoal, yGoal));
}
public void AddPose(RobotPose p)
{
lock (poses)
{
poses.Add(p);
if (poses.Count > maxPoints)
{
poses.RemoveAt(0);
}
}
}
public void Enqueue(RobotPose pose)
{
lock (this)
{
if (poseQueue.Count >= queueLength)
{
poseQueue.Dequeue();
}
poseQueue.Enqueue(pose);
}
}
public void SetScan(ILidarScan <ILidar2DPoint> scan, RobotPose robotPose)
{
lock (this.drawLock)
{
this.scan = scan;
this.time = scan.Timestamp;
this.numPoints = scan.Points.Count;
this.robotPose = robotPose;
this.startIdx = 0;
this.endIdx = scan.Points.Count - 1;
}
}
// see http://code.jonwagner.com/2012/09/06/best-practices-for-c-asyncawait/
// http://www.c-sharpcorner.com/UploadFile/pranayamr/difference-between-await-and-continuewith-keyword-in-C-Sharp/
// http://code.jonwagner.com/2012/09/04/deadlock-asyncawait-is-not-task-wait/
// https://msdn.microsoft.com/en-us/library/ff963550.aspx - Parallel Programming book
public override async Task Init(CancellationTokenSource cts, string[] args)
{
await base.Init(cts, args); // produces hardwareBrick and sets it for communication
joystick.joystickEvent += Joystick_joystickEvent;
sensorsController = new SensorsControllerShorty(hardwareBrick, loopTimeMs, PIXY_COM_PORT, speaker);
await sensorsController.InitSensors(cts);
InitDrive();
behaviorFactory = new BehaviorFactory(subsumptionTaskDispatcher, driveGeometry, speaker);
// we can set behavior combo now, or allow ControlDeviceCommand to set it later.
//behaviorFactory.produce(BehaviorCompositionType.JoystickAndStop);
robotState = new RobotState();
robotPose = new RobotPose();
robotState.powerLevelPercent = 100; // can be changed any time. Used by behaviors.
robotPose.geoPosition.moveTo(lng, lat, elev); // will be set to GPS coordinates, if available
robotPose.direction.heading = headingDegrees; // will be set to Compass Heading, if available
robotPose.reset();
robotSlam = new RobotSLAM()
{
useOdometryHeading = true
};
await InitComm(cts); // may throw exceptions
// see what kind of timer we have to measure durations:
if (Stopwatch.IsHighResolution)
{
Debug.WriteLine("OK: operations are timed using the system's high-resolution performance counter.");
}
else
{
Debug.WriteLine("Warning: operations are timed using the DateTime class.");
}
stopWatch.Start();
if (isCommError)
{
speaker.Speak("Shorty cannot see his brick");
}
else
{
speaker.Speak("I am Shorty");
}
}
public void SetScan(ILidarScan <ILidar2DPoint> scan, RobotPose robotPose, int startIdx, int endIdx)
{
lock (this.drawLock)
{
this.scan = scan;
this.time = scan.Timestamp;
this.numPoints = endIdx - startIdx + 1;
this.robotPose = robotPose;
this.startIdx = startIdx;
this.endIdx = endIdx;
}
}
/// <summary>
/// Update robot to global coordination DCM with a Pose
/// </summary>
/// <param name="robotPosition"></param>
public void UpdatePose(RobotPose robotPosition)
{
this.currentRobotPose = robotPosition;
Matrix4 robot2GlocalDCM = Matrix4.FromPose(robotPosition);
for (int i = 0; i < 4; i++)
{
for (int j = 0; j < 4; j++)
{
robotToGlocalDCM[i, j] = robot2GlocalDCM[i, j];
}
}
}
/// <summary>
/// Get difference of two occupancy map
/// </summary>
/// <param name="globalMulti">ocGrid1</param>
/// <param name="globalSingle">ocGrid2</param>
/// <param name="currentPose">current position</param>
/// <param name="extentX">x-length of the comparison</param>
/// <param name="extentY">y-length of the comparison</param>
/// <returns>list of index classes - has ocGrid1 value</returns>
public UpdateMapDataMessage Diff(int robotID, OccupancyGrid2D globalMulti, RobotPose currentPose, double extentX, double extentY)
{
if (!globalOcGridByEachRobotAlgorithm.ContainsKey(robotID))
{
return(null);
}
OccupancyGrid2D globalSingle = globalOcGridByEachRobotAlgorithm[robotID].UhatGM;
//OccupancyGrid2D globalSingle = globalOcGridByEachRobot[robotID];
//List<Index> diffIndexToSend = new List<Index>();
List <Position> diffPositionToSend = new List <Position>();
//List<float> heightList = new List<float>();
//List<float> covList = new List<float>();
//List<float> pijList = new List<float>();
List <float> pijSum = new List <float>();
List <float> puHat = new List <float>();
List <float> puHatSquare = new List <float>();
List <float> pSigUhateSquare = new List <float>();
int numCellXHalf = (int)(extentX / globalMulti.ResolutionX);
int numCellYHalf = (int)(extentY / globalMulti.ResolutionY);
int currentCellX, currentCellY;
globalMulti.GetIndicies(currentPose.x, currentPose.y, out currentCellX, out currentCellY);
int comparisonCellX, comparisonCellY;
for (int i = 0; i < numCellYHalf * 2; i++) // [i, j] = [column, row]
{
for (int j = 0; j < numCellXHalf * 2; j++)
{
comparisonCellX = currentCellX - numCellXHalf + j;
comparisonCellY = currentCellY - numCellYHalf + i;
if (globalMulti.GetCellByIdx(comparisonCellX, comparisonCellY) != globalSingle.GetCellByIdx(comparisonCellX, comparisonCellY))
{
double x, y; globalMulti.GetReals(comparisonCellX, comparisonCellY, out x, out y);
diffPositionToSend.Add(new Position((float)x, (float)y));
//heightList.Add((float)gaussianMixMapAlgorithm.UhatGM.GetCellByIdx(j, i));
//covList.Add((float)gaussianMixMapAlgorithm.Psig_u_hat_square.GetCellByIdx(j, i));
//pijList.Add((float)gaussianMixMapAlgorithm.Pij_sum.GetCellByIdx(j, i));
pijSum.Add((float)gaussianMixMapAlgorithm.Pij_sum.GetCell(x, y));
puHat.Add((float)gaussianMixMapAlgorithm.Pu_hat.GetCell(x, y));
puHatSquare.Add((float)gaussianMixMapAlgorithm.Pu_hat_square.GetCell(x, y));
pSigUhateSquare.Add((float)gaussianMixMapAlgorithm.Psig_u_hat_square.GetCell(x, y));
}
}
}
return(new UpdateMapDataMessage(robotID, diffPositionToSend, pijSum, puHat, puHatSquare, pSigUhateSquare));
}
public void Draw(Renderer cam)
{
if (poses.Count == 0)
{
return;
}
lock (poses)
{
RobotPose lastPose = poses[0];
foreach (RobotPose p in poses)
{
GLUtility.DrawCross(pen, p.ToVector2(), .2f);
//GLUtility.DrawLine(pen, lastPose.ToVector2(), p.ToVector2());
lastPose = p;
}
if (poses[poses.Count - 1].covariance[0, 0] != 0)
{
//calculate the error ellipse
Matrix cov = new Matrix(2, 2);
cov[0, 0] = poses[poses.Count - 1].covariance[0, 0];
cov[0, 1] = poses[poses.Count - 1].covariance[0, 1];
cov[1, 0] = poses[poses.Count - 1].covariance[1, 0];
cov[1, 1] = poses[poses.Count - 1].covariance[1, 1];
double theta = .5 * Math.Atan2((-2 * cov[0, 1]), (cov[0, 0] - cov[1, 1]));
if (Double.IsNaN(theta))
{
theta = 0;
}
double sigu2 = (cov[0, 0] * Math.Sin(theta) * Math.Sin(theta)) +
(2 * cov[0, 1] * Math.Sin(theta) * Math.Cos(theta)) +
(cov[1, 1] * Math.Cos(theta) * Math.Cos(theta));
double sigv2 = (cov[0, 0] * Math.Cos(theta) * Math.Cos(theta)) +
(2 * cov[0, 1] * Math.Sin(theta) * Math.Cos(theta)) +
(cov[1, 1] * Math.Sin(theta) * Math.Sin(theta));
GLUtility.GoToTransformXYZ((float)poses[poses.Count - 1].x, (float)poses[poses.Count - 1].y, 0);
GLUtility.GoToTransformYPR((float)theta, 0, 0);
GLUtility.DrawEllipse(ellipsePen, new RectangleF((float)-sigu2 / 2, (float)-sigv2 / 2, (float)sigu2, (float)sigv2));
GLUtility.ComeBackFromTransform();
GLUtility.ComeBackFromTransform();
}
}
}
public RobotKeypoint(int id)
{
color = Color.Black;
this.id = id;
name = "Robot " + id;
runningPath = new PathToRobotRenderer();
commandedPath = new PathToRobotRenderer();
pose = new RobotPose();
origPlygn = new Polygon();
origPlygn.Add(new Vector2(.25, .36));
origPlygn.Add(new Vector2(.25, -.36));
origPlygn.Add(new Vector2(-.25, -.36));
origPlygn.Add(new Vector2(-.25, .36));
bodyPlygn = new Polygon(origPlygn.points);
}
public NewPoseAvailableEventArgs(RobotPose pose)
{
this.pose = pose;
}
public RobotPose ToRobotPose()
{
//Extract the positions
double x = this[0, 3];
double y = this[1, 3];
double z = this[2, 3];
double pitch = Math.Asin(this[0, 2]);
double roll; double yaw;
if (Math.Abs(this[0, 2]) <= 1) // cos(y) != 0 Gimbal-Lock
{
roll = Math.Atan2(-this[1, 2], this[2, 2]);
yaw = Math.Atan2(-this[0, 1], this[0, 0]);
}
else
{
Console.WriteLine("gimbal lock");
roll = 0;
yaw = Math.Atan2(-this[1, 0], this[1, 1]);
}
RobotPose p = new RobotPose(x, y, z, yaw, pitch, roll, 0);
return p;
}
public RobotPoseMessage(int robotID, RobotPose pose)
{
this.robotID = robotID; this.pose = pose;
}
// see http://code.jonwagner.com/2012/09/06/best-practices-for-c-asyncawait/
// http://www.c-sharpcorner.com/UploadFile/pranayamr/difference-between-await-and-continuewith-keyword-in-C-Sharp/
// http://code.jonwagner.com/2012/09/04/deadlock-asyncawait-is-not-task-wait/
// https://msdn.microsoft.com/en-us/library/ff963550.aspx - Parallel Programming book
public override async Task Init(CancellationTokenSource cts, string[] args)
{
await base.Init(cts, args); // produces hardwareBrick and sets it for communication
joystick.joystickEvent += Joystick_joystickEvent;
sensorsController = new SensorsControllerShorty(hardwareBrick, loopTimeMs);
sensorsController.InitSensors(cts);
InitDrive();
behaviorFactory = new BehaviorFactory(subsumptionTaskDispatcher, driveGeometry, speaker);
// we can set behavior combo now, or allow ControlDeviceCommand to set it later.
//behaviorFactory.produce(BehaviorCompositionType.JoystickAndStop);
robotState = new RobotState();
robotPose = new RobotPose();
robotState.powerLevelPercent = 100; // can be changed any time. Used by behaviors.
robotPose.geoPosition.moveTo(lng, lat, elev); // will be set to GPS coordinates, if available
robotPose.direction.heading = headingDegrees; // will be set to Compass Heading, if available
robotPose.resetXY();
robotSlam = new RobotSLAM();
await InitComm(cts); // may throw exceptions
// see what kind of timer we have to measure durations:
if (Stopwatch.IsHighResolution)
{
Debug.WriteLine("OK: operations are timed using the system's high-resolution performance counter.");
}
else
{
Debug.WriteLine("Warning: operations are timed using the DateTime class.");
}
stopWatch.Start();
if (isCommError)
{
speaker.Speak("Shorty cannot see his brick");
}
else
{
speaker.Speak("I am Shorty");
}
}
public void Enqueue(RobotPose pose)
{
lock (this)
{
if (poseQueue.Count >= queueLength)
poseQueue.Dequeue();
poseQueue.Enqueue(pose);
}
}
