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