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); } }