//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! }