//this function checks a turn angle (provided by wheel scans) to see if there is anything in the way of the desired turn angle.
//It returns true if the robot can turn that direction without hitting an obstacle, and returns false if there is something in
//the way of the entered turn angle.
static bool combinedVectorScan(LocationPoint source, LocationPoint destination)
{
double target_angle = adjust_angle(Math.Atan2(destination.X - source.X, destination.Y - source.Y), 2.0*Math.PI); //limit angle between (-pi & pi) or (-180 & 180)
double target_dist = destination.DistanceBetween(source); //find distance between corner of wheel to desired target. this equals Buffer length
double dist;
int i = 0, j = 0;//set up two iterators
double tempDist = 0.0;
LocationPoint sample_point= new LocationPoint();// create temporary point which is updated each iteration
do// find out if there is anything in the way of the
{
sample_point.X = (i * SAMPLING_DISTANCE) * Math.Sin(target_angle);//create temporary XY point along the desired target angle at varying distances
sample_point.Y = (i * SAMPLING_DISTANCE) * Math.Cos(target_angle);//create temporary XY point along the desired target angle at varying distances
foreach (var cpoint in combinedBufPoints)//look through each point point in the buffer LiDAR points to see if any are by the temporary point
{
if (cpoint.Y < 0) //if it's behind the robot, skip it
{
continue;
}
dist = sample_point.DistanceBetween(cpoint); //figure out distance between the temporary point and this point in the buffer LiDAR points
if (dist < combinedBufferWidth) // if the dcurrent LiDAR point is close to the temporary point
{
badCount++;//increment the number of LiDAR points in the way of this target angle
if (badCount > BADCOUNTTHRESH) //if there's too many lidar points in the way then this is not a valid turn angle
{
return false;//return false to indicate that this turn angle is not good.
}//end BAD COUNT THRESH
}//END dist<combinedBufferWidth
}//end For each LiDAR point
i++;
} while (SAMPLING_DISTANCE * i < target_dist); // while the sampling distance along the target angle is less than the target distance
return true;//return true when it is possible for the robot to take this turn angle without hitting obstacles
}//end CombinedVectorScan
}//end determine robot location
//Scan landmarks is used to find all of the surrounding landmark locations. To do this the LiDAR data is needed, the list of known landmark locations
//from a text file are needed, and the robots current location (to convert each point in the LiDAR data to an XY coordinate in the field).
public static List <LocationPoint> ScanLandmarks(List <LidarPoint> LMSdata, List <LidarPoint> KLM, LocationPoint robotLocation)
{
var currentLandmarks = new List <LocationPoint>(); //anytime a landmark is found in a scan the landmark is saved to this arrray
foreach (var knownLandmark in KLM) //loop through each known landmark in the list of imported landmarks
{
//smallestSeperation is the maximum distance a point can be from where a landmark should be to be considered part of the landmark
//double smallestSeparation = 0.55; ////landmark_tolerance;
double smallestSeparation = 0.45; ////landmark_tolerance;
//double smallestSeparation = 1; ////landmark_tolerance;
//double smallestSeparation = 1.75;
LocationPoint matchedLandmark = null;// create a point to match the landmark too if it is found
//The following variables must be reset to zero at the beginning of each landmark search.
double Xsum = 0, ysum = 0, headingsum = 0; //everytime a point is found to be part of a landmark the X, Y heading
// are summed together, then divded by number of points found to find the X, Y and heading averages for the points associated with the landmark
int found_points = 0; //save the number of points that become associated with the landmark
int twiceDist_points = 0; //used for debuging
//loop through every point in the LiDAR scan
foreach (var currentPoint in LMSdata)
{
if (currentPoint == null) //check if the LiDAR point did not return because there was nothing for the laser to bounce off of
{
continue; //forget about this point
}
var landmarkLocationPoint = currentPoint.ToLocationPoint(robotLocation); // convert the current point we are looking at
//into a point in the XY field based off of where the point is in the scan and where the robot is in the field.
// figure out the difference between where the landmark should be and where the landmark is in the X coordinate
double xSeparation = knownLandmark.X - landmarkLocationPoint.X;
// figure out the difference between where the landmark should be and where the landmark is in the Y coordinate
double ySeparation = knownLandmark.Y - landmarkLocationPoint.Y;
// figure out the distance between the LiDAR point we are looking at compared to where
double currentSeparation = Math.Sqrt(Math.Pow(xSeparation, 2) + Math.Pow(ySeparation, 2));
//the landmark should be
// if the seperation is beneath the set tolerance. IE the point is close enough to the landmarks expected position to be considered part
if (currentSeparation < smallestSeparation)
//of the landmark
{
Xsum += currentPoint.X; //add the point to the Xsum
ysum += currentPoint.Y; //add the point to the Ysum
headingsum += currentPoint.Heading; //add the point to the heading sum
found_points++; //increment the number of points found
}
}//end loop through LiDAR points
//now check if the points which were associated with the landmark were enough to be considered a landmark
// if the number of points is low (less than thresh), then this likely isnt a landmark and it should not be assumed that this is a landmark
if (found_points > LM_POINTS_THRESH)
{
//This is considered a landmark!
double XAverage = Xsum / (double)found_points; //find average of all X values of the points associated with the landmark
double YAverage = ysum / (double)found_points; //find average of all Y values of the points associated with the landmark
double headingAverage = headingsum / (double)found_points; //find average of all heading values of the points associated with the landmark
double distanceAverage = Math.Sqrt(XAverage * XAverage + YAverage * YAverage); //find average distance from the robot to the landmark point
if (distanceAverage < 15) ////MAX_LANDMARKS_DISTANCE)// check if the landmark is less than 15M away
{
matchedLandmark = new LocationPoint(); // we have a new landmark!
matchedLandmark.X = knownLandmark.X; //save the landmarks X value
matchedLandmark.Y = knownLandmark.Y; //save the landmarks Y value
//the next part is saving the distance between the robot and the landmark.
//40% is the distance average, and 60% is the distance from the point & the robot
//this is done because neither of the distance vaues are exactly correct due to the fact that
//the landmark locations are not exactly at the center of the landmark(as they should be)
//so it is assumed that the distance from the selected point is slightly more correct
//than the distance average(which is the sum of the distancese from all of the found points)
//40% of the distance from the landmark is the distance average
matchedLandmark.Distance = 0.4 * distanceAverage;
//60% of the distance is the distance from the found landmarks position from the robot
matchedLandmark.Distance += 0.6 * robotLocation.DistanceBetween(matchedLandmark);
matchedLandmark.Heading = headingAverage; // heading average is saved
currentLandmarks.Add(matchedLandmark); //yay we have a new landmark so add it to our list of landmarks!
continue;
}
}
}//end for each landmark loops
return(currentLandmarks);//yay here are all of our landmarks!
}