private void parseData(LidarPoint lidarpoint) { int angle = lidarpoint.mAngle; int length = lidarpoint.mDistance; //float angle = N ["angle"].AsFloat; //float length = N ["length"].AsFloat; float radians = angle * (Mathf.PI / 180); float x = length * Mathf.Sin(radians); float y = length * Mathf.Cos(radians); float z = 0.0f; float new_x = transform.position.x + x; new_x = new_x / scale; float new_y = transform.position.y + y; new_y = new_y / scale; float new_z = transform.position.z + z; Vector3 locationVector3 = new Vector3(new_x, new_y, new_z); float distance = Vector3.Distance(transform.position, locationVector3); if (distance >= 50) { transform.position = locationVector3; } }
//parse data IEnumerator ParseData(string data) { Queue lidarQueue = new Queue(); LidarPoint newPoint; //translate r,0;r,0;...|metadata into pairs into LidarPoint Struct string[] dataSplit = data.Split('|');//[LidarData(r,0;r,0;...),metadata] string metaData = dataSplit[1]; Console.Write("Meta Data: " + metaData); string[] dataBunch = dataSplit[0].Split(';');//["r,0","r,0"....] string[] dataPair; int id = 0;//for id //process pairs foreach (string s in dataBunch) { dataPair = s.Split(','); //[distance,angle] newPoint = new LidarPoint(id + " :: " + metaData, dataPair[0], dataPair[1]); lidarQueue.Enqueue(newPoint); // puts LidarPoint on the queue id++; } yield return(DrawPoints(lidarQueue));//draw points from queue }
}//end Combined Right Wheel Scan public static double combinedLeftWheelScan(LidarPoint target)// see right wheel scan for comments { LocationPoint source = new LocationPoint(); double target_angle = adjust_angle(Math.Atan2(target.X, target.Y), 2*Math.PI);//this function limits the angle between (-pi & pi) or (-180 & 180) double sample_phi; int i = 0; source.X = -combinedHalfRobotWidth; source.Y = 0.0; do { LocationPoint sample_point = new LocationPoint(); sample_point.X = combinedBufferLength * Math.Sin(target_angle + SAMPLING_ANGLE * i); sample_point.Y = combinedBufferLength * Math.Cos(target_angle + SAMPLING_ANGLE * i); sample_phi = adjust_angle(Math.Atan2(sample_point.X, sample_point.Y), 2*Math.PI);//this function limits the angle between (-pi & pi) or (-180 & 180) if (combinedVectorScan(source, sample_point)) { if (combinedCheckAngle(sample_phi)) { if (i == 0) { return 0; } return sample_phi; } } i++; } while (i < ANGLE_SAMPLES); // return DOOM; return DOOM; }
private void parseData(LidarPoint lidarpoint) { int angle = lidarpoint.mAngle; int length = lidarpoint.mDistance; //float angle = N ["angle"].AsFloat; //float length = N ["length"].AsFloat; float radians = angle * (Mathf.PI / 180); float x = length * Mathf.Sin (radians); float y = length * Mathf.Cos (radians); float z = 0.0f; float new_x = transform.position.x + x; new_x = new_x / scale; float new_y = transform.position.y + y; new_y = new_y / scale; float new_z = transform.position.z + z; Vector3 locationVector3 = new Vector3 (new_x, new_y, new_z); float distance = Vector3.Distance (transform.position, locationVector3); if (distance >= 50){ transform.position = locationVector3; } }
/// <summary> /// Create a new R2000Scanner object given the scanner's IP address and /// the connection type used to stream LIDAR data /// </summary> /// <param name="address">IP address of the scanner</param> /// <param name="connectionType">TCP or UDP for data streaming</param> /// <param name="fetchStatusInterval">Interval to fetch the scanner status. Set to 0 to disable</param> public R2000Scanner(IPAddress address, R2000ConnectionType connectionType, int fetchStatusInterval = 10000) { this.fetchStatusInterval = fetchStatusInterval; // retrieve basic info commandClient = new HttpClient(); commandClient.BaseAddress = new Uri($"http://{address.ToString()}/cmd/"); if (connectionType == R2000ConnectionType.TCPConnection) { this.dataStreamConnector = new TCPConnector(commandClient, address, true, 10000); } else if (connectionType == R2000ConnectionType.UDPConnection) { throw new NotImplementedException(); } var latestNativeScanCounter = 0; // publish the points ScanFramePoint lastValidPoint = new ScanFramePoint(); this.dataStreamConnector.Subscribe <ScanFramePoint>(pt => { // we don't use the native R2000 scan counter at it wraps around quite quickly // thus, we store the "latestNativeCounter" and check for increment or wraparound if (pt.ScanCounter > latestNativeScanCounter || pt.ScanCounter < latestNativeScanCounter) { latestNativeScanCounter = pt.ScanCounter; base.ScanCounter++; } //// don't publish invalid packets if (!pt.Valid) { pt = lastValidPoint; } else { lastValidPoint = pt; } // return; //var carthCoordinate = base.TransfromScannerToSystemCoordinates(pt.Angle, pt.Distance); System.Numerics.Vector3 carthCoordinate = new System.Numerics.Vector3(0); var point = new LidarPoint( carthCoordinate, pt.Angle, pt.Distance, (byte)(pt.Amplitude >> 4), //(byte)(pt.Amplitude / 256), Valeur max 4096 -> ramené à 256 base.ScanCounter, this); PublishLidarEvent(point); }); // redirect the error/status events this.dataStreamConnector.Subscribe <LidarStatusEvent>(ev => this.PublishLidarEvent(ev)); }
// Use this for initialization // Update is called once per frame void Update() { if (queue.Count > 0) { LidarPoint lidarpoint = (LidarPoint)queue.Dequeue(); parseData(lidarpoint); } }
public static LocationPoint ToLocationPoint(this LidarPoint initialPoint, LocationPoint robotLocation) { double headingValue = initialPoint.Heading + robotLocation.Heading; double xValue = robotLocation.X + initialPoint.Distance * Math.Sin(headingValue); double yValue = robotLocation.Y + initialPoint.Distance * Math.Cos(headingValue); return(new LocationPoint(xValue, yValue, headingValue)); }
public LidarPoint Intersect(LinearEquation equation2) { if (this.Slope == equation2.Slope) { throw new InvalidOperationException("Equations are parallel."); } double X = -(equation2.Intercept - this.Intercept) / (equation2.Slope - this.Slope); double Y = ((this.Intercept * equation2.Slope) - (equation2.Intercept * this.Slope)) / (equation2.Slope - this.Slope); var pointToReturn = new LidarPoint(X, Y); return(pointToReturn); }
public bool tryAddPoint(LidarPoint p) { if (p.DistanceBetween(midpoint) <= maxDistanceFromMidpoint) { my_midpoint = new LidarPoint((points.Count() * midpoint.X + p.X) / (points.Count() + 1), (points.Count() * midpoint.Y + p.Y) / (points.Count() + 1)); points.Add(p); return(true); } else { return(false); } }
//Threaded portion for recieving and preprocessing the data. private void ReceiveData() { client = new UdpClient(port); while (stop_send == false) { try { IPEndPoint anyIP = new IPEndPoint(IPAddress.Any, 0); byte[] data = client.Receive(ref anyIP); string text = Encoding.UTF8.GetString(data); LidarPoint lidarpoint = convertData(text); queue.Enqueue(lidarpoint); } catch (Exception err) { print(err.ToString()); } } }
public static List <LidarPoint> KnownLandmarksInit(string fileName) { float tempx, tempy; int u = 0; StreamReader landmarkFile = new StreamReader(fileName); List <LidarPoint> knownLandmarks = new List <LidarPoint>(); while (!landmarkFile.EndOfStream) { string currentLine = landmarkFile.ReadLine(); string[] currentLineStrings = currentLine.Split(new [] { ' ' }, StringSplitOptions.RemoveEmptyEntries); double currentXValue = Convert.ToDouble(currentLineStrings[0]); double currentYValue = Convert.ToDouble(currentLineStrings[1]); var currentLidarPoint = new LidarPoint(currentXValue, currentYValue); knownLandmarks.Add(currentLidarPoint); } landmarkFile.Close(); return(knownLandmarks); ////FILE *fp; ////if ((fp = fopen(fileName, "r")) == null) { //// fprintf(stderr, "Could not open landmark file\n"); ////} ////numLandmarks = 0; ////while (!feof(fp)) { //// fscanf(fp, "%f %f", &tempx, &tempy); //// KLM[u].x = tempx*M2MM; //// KLM[u].y = tempy*M2MM; //// KLM[u].found = false; //// KLM[u].rejected = false; //// KLM[u].phi = atan2(KLM[u].x, KLM[u].y); //// KLM[u].dist=D(KLM[u].x, KLM[u].y); //// u++; ////} ////num_landmarks = u; ////fclose(fp); }
public LinearEquation(LidarPoint point1, LidarPoint point2, bool perpendicular = false) { midpoint = new LidarPoint((point1.X + point2.X) / 2, (point1.Y + point2.Y) / 2); endpoint1 = point1; endpoint2 = point2; double pointSlope = (point2.Y - point1.Y) / (point2.X - point1.X); if (perpendicular) { Slope = -(1 / pointSlope); } else { Slope = pointSlope; } Intercept = midpoint.Y - (Slope * midpoint.X); }
//On each frame, read from the Lidar Queue //Then if there is data call parseData void Loop() { if (queue.Count >= 500) { for (int i = 0; i < 360; i++) { if (queue.Count > 0) { try { LidarPoint lidarpoint = (LidarPoint)queue.Dequeue(); parseData(lidarpoint); } catch { break; } } } } }
//draw lidar point objects IEnumerator DrawPoints(Queue lidarQueue) { while (lidarQueue.Count > 0) { LidarPoint lidarPoint = (LidarPoint)lidarQueue.Dequeue(); int angle = lidarPoint.mAngle; int length = lidarPoint.mDistance; //float angle = N ["angle"].AsFloat; //float length = N ["length"].AsFloat; float radians = angle * (Mathf.PI / 180); float x = length * Mathf.Sin(radians); float y = length * Mathf.Cos(radians); float z = 0.0f; float new_x = transform.position.x + x; new_x = new_x / scale; float new_y = transform.position.y + y; new_y = new_y / scale; float new_z = transform.position.z + z; Vector3 locationVector3 = new Vector3(new_x, new_y, new_z); if (pointObjects[angle]) { float distance = Vector3.Distance(pointObjects[angle].transform.position, locationVector3); if (distance >= 50) { pointObjects[angle].transform.position = locationVector3; } } else { Instantiate(pointObjectPrefab, locationVector3, transform.rotation); } } okToRecieve = true; yield return(0); }
}//end combined Check angle //This function searched for a right turn angle which the robot can avoid an obstacle to the right. // The function starts out by looking at the target point in, and finds a turn to the right of that angle, not to the right of straight forward. public static double combinedRightWheelScan(LidarPoint target) { LocationPoint source = new LocationPoint();// source is the point at the front right corner of the right wheel double target_angle = adjust_angle(Math.Atan2(target.X, target.Y), 2.0*Math.PI); // this is the desired angle to the target point, //this function limits the angle between (-pi & pi) or (-180 & 180) double sample_phi; // temporary variable which holds various angles to look at int i = 0;// set up iterator to initailize at 0 source.X = combinedHalfRobotWidth;// assigning source point to the front right corner of the right wheel source.Y = 0; //assigning the source point to a value of Y=0. do// find the closest turn angle on the right of the target turn angle which will avoid obstacles { LocationPoint sample_point = new LocationPoint(); // create a new point which might be the point we can go towards sample_point.X = combinedBufferLength * Math.Sin(target_angle - SAMPLING_ANGLE * i);//assign the sample point X Value sample_point.Y = combinedBufferLength * Math.Cos(target_angle - SAMPLING_ANGLE * i);//assign sample point Y Value sample_phi = adjust_angle(Math.Atan2(sample_point.X, sample_point.Y), 2.0 * Math.PI);//this function limits the angle between (-pi & pi) or (-180 & 180) //check if there is anything in the way of this turn angle. Combined Vector Scan will return //true when it is possible for the robot to take the input turn angle, and false if there is something in the way. if (combinedVectorScan(source, sample_point)) { //the turn angle of sample_phi is safe for this wheel if (combinedCheckAngle(sample_phi)) //double check this turn angle is safe for both wheels { if (i == 0)//if this is the first itration (IE the entered desired turn angle is safe), then return 0. { return 0; } return sample_phi;//otherwise return the turn angle needed to avoid the obstacle }//end Combined Check ANgle }//end COmbined Vector Scan i++; } while (i < ANGLE_SAMPLES); return DOOM;//if it is impossible to avoid an obstacle then return doom which is a large turn angle.... }//end Combined Right Wheel Scan
private void parseData(LidarPoint lidarpoint) { int angle = lidarpoint.mAngle; int length = lidarpoint.mDistance; //float angle = N ["angle"].AsFloat; //float length = N ["length"].AsFloat; float radians = angle * (Mathf.PI / 180); float x = length * Mathf.Sin(radians); float y = length * Mathf.Cos(radians); float z = 0.0f; float new_x = transform.position.x + x; new_x = new_x / scale; float new_y = transform.position.y + y; new_y = new_y / scale; float new_z = transform.position.z + z; Vector3 locationVector3 = new Vector3(new_x, new_y, new_z); //string name = (string)lidarpoint.Id.ToString () + lidarpoint.X.ToString (); /* * if (length < 700 && length > 30){ * send_queue.Enqueue(length.ToString()); * } */ numberofChildren = transform.childCount; totalPoints += 1; childrenSweep += 1; if (length == 0) { badDataPoint += 1; } if (childrenSweep >= numberofChildren) { currentTime = Time.time - lastTime; childrenSweep = 0; lastTime = Time.time; } GameObject pointInstance = pointArray[angle]; float normal = pointInstance.GetComponent <pointNormal>().getNormal(); if (normal == 0.0f) { pointInstance.transform.position = locationVector3; pointInstance.GetComponent <Renderer>().enabled = true; pointInstance.GetComponent <Collider>().enabled = true; } else if ((length / scale >= normal * .90) && (length / scale <= normal * 1.10)) { pointInstance.GetComponent <Renderer>().enabled = false; pointInstance.GetComponent <Collider>().enabled = false; } else { pointInstance.transform.position = locationVector3; pointInstance.GetComponent <Renderer>().enabled = true; pointInstance.GetComponent <Collider>().enabled = true; } }
public void addtoQueue(LidarPoint lidarpoint) { queue.Enqueue(lidarpoint); }
public intersectBucket(LidarPoint firstPoint) { points = new List <LidarPoint>(); points.Add(firstPoint); my_midpoint = firstPoint; }
private void AddPoint(LidarPoint p) { my_midpoint = new LidarPoint((points.Count() * midpoint.X + p.X) / (points.Count() + 1), (points.Count() * midpoint.Y + p.Y) / (points.Count() + 1)); points.Add(p); }
private void parseData(LidarPoint lidarpoint) { int angle = lidarpoint.mAngle; int length = lidarpoint.mDistance; //float angle = N ["angle"].AsFloat; //float length = N ["length"].AsFloat; float radians = angle * (Mathf.PI / 180); float x = length * Mathf.Sin (radians); float y = length * Mathf.Cos (radians); float z = 0.0f; float new_x = transform.position.x + x; new_x = new_x / scale; float new_y = transform.position.y + y; new_y = new_y / scale; float new_z = transform.position.z + z; Vector3 locationVector3 = new Vector3 (new_x, new_y, new_z); //string name = (string)lidarpoint.Id.ToString () + lidarpoint.X.ToString (); /* if (length < 700 && length > 30){ send_queue.Enqueue(length.ToString()); } */ numberofChildren = transform.childCount; totalPoints += 1; childrenSweep += 1; if (length == 0){ badDataPoint += 1; } if (childrenSweep >= numberofChildren){ currentTime = Time.time - lastTime; childrenSweep = 0; lastTime = Time.time; } GameObject pointInstance = pointArray[angle]; float normal = pointInstance.GetComponent<pointNormal>().getNormal(); if (normal == 0.0f){ pointInstance.transform.position = locationVector3; pointInstance.GetComponent<Renderer>().enabled = true; pointInstance.GetComponent<Collider>().enabled = true; } else if((length / scale >= normal * .90) && (length / scale <= normal * 1.10)){ pointInstance.GetComponent<Renderer>().enabled = false; pointInstance.GetComponent<Collider>().enabled = false; } else { pointInstance.transform.position = locationVector3; pointInstance.GetComponent<Renderer>().enabled = true; pointInstance.GetComponent<Collider>().enabled = true; } }
private void ProcessLidarScanFrames() { byte[] buffer; Queue<byte> potentialFrame = new Queue<byte>(7); Queue<byte> scan = new Queue<byte>(); Action refillScanBuffer = () => { if (rxScanBuffer.Count > 0) { var s = rxScanBuffer.Dequeue(); foreach (var x in s) scan.Enqueue(x); } else Thread.Sleep(1); }; while (true) { // cleanup potentialFrame.Clear(); // get the next 7 bytes while (potentialFrame.Count < 7) { if (scan.Count == 0) refillScanBuffer(); else potentialFrame.Enqueue(scan.Dequeue()); // get a byte // exit point if (scanProcessingCts.IsCancellationRequested) return; } if (!ChecksumIsValid(potentialFrame)) { // garbage... :S DiscardedFrames++; var localDiscardedBytes = 0; // find a whole frame do { // trow away one byte and get the next instead // ...until we get something meaningful if (scan.Count == 0) refillScanBuffer(); else { localDiscardedBytes++; potentialFrame.Dequeue(); potentialFrame.Enqueue(scan.Dequeue()); } // exit point if (scanProcessingCts.IsCancellationRequested) return; } while (!ChecksumIsValid(potentialFrame)); this.DiscardedBytes += localDiscardedBytes; PublishLidarEvent(new LidarStatusEvent($"Checksum error, had to discard {localDiscardedBytes} bytes to recover to a valid read window", LidarStatusLevel.Warning)); } buffer = potentialFrame.ToArray(); // extract data var errorSync = buffer[0]; // skip on error packets if ((errorSync & (1 << 1)) == 1) { PublishLidarEvent(new LidarStatusEvent("Communication error with LIDAR module (Sweep error bit E0)", LidarStatusLevel.Error)); continue; } // check if this is a sync packet if ((errorSync & 0x1) == 1) this.ScanCounter++; // get coordinates and signal amplitude var azimuth = (buffer[1] + (buffer[2] << 8)) / 16.0f; var distance = (buffer[3] + (buffer[4] << 8)) * 10; // convert cm to mm var signal = buffer[5]; // discard short range points if (distance < this.MinPointDistance) continue; // transform the polar to carthesian coordinates within the applications coordinate // system (not scanner centric) var carthesianPoint = base.TransfromScannerToSystemCoordinates(azimuth, distance); // exit point if (scanProcessingCts.IsCancellationRequested) return; // propagate the new lidar point through the system var scanPacket = new LidarPoint(carthesianPoint, azimuth, distance, signal, this.ScanCounter, this); PublishLidarEvent(scanPacket); } }