Detect obstacles using the RPLidar Sensor (and the GBLIDAR Class from here: https://gist.github.com/OzTamir/ddf8d856ec03b723d273)

ObstaclesDetector.cpp

#include "ObstaclesDetector.h"

using namespace GBCode;

ObstaclesDetector::ObstaclesDetector(double distanceThreshold, double resolution)
{
	this->distanceThreshold = distanceThreshold;
	this->resolution = resolution;

	lidar = new GBLIDAR();
	lidar->startScan();
}

/*
	Check whether there is an obstacle in the given range
	@ TODO: Input validation
*/
ObstaclesDetector::DetectionResult ObstaclesDetector::obstaclesInRange(double startAngle, double endAngle) {
	int foundCount = 0; int notFoundCount = 0; int errorCount = 0;
	// Make an initial scan to create the cache
	lidar->GetDistanceForAngle(startAngle, false);
	while (startAngle <= endAngle)
	{
		switch (obstacleInAngle(startAngle))
		{
			case kFound:
				foundCount++;
				break;
			case kNotFound:
				notFoundCount++;
				break;
			default:
				errorCount++;
				break;
		}
		startAngle += resolution;
	}

	if (foundCount >= notFoundCount && foundCount >= errorCount)
		return kFound;
	if (notFoundCount >= errorCount && notFoundCount >= foundCount)
		return kNotFound;
	return kError;

}

/*
	Check wheter the distance for a given angle is small enough to count as an obstacle
*/
ObstaclesDetector::DetectionResult ObstaclesDetector::obstacleInAngle(double angle) {
	double distance = lidar->GetDistanceForAngle(angle, true);
	if (distance == 0)
		return kError;
	if (distance < distanceThreshold)
		return kFound;
	return kNotFound;

}

/*
	Setter for this->distanceThreshold
*/
void ObstaclesDetector::setDistanceThreshold(double newThresh) {
	distanceThreshold = newThresh;
}

/*
	Setter for this->resolution
*/
void ObstaclesDetector::setResolution(double newResolution) {
	resolution = newResolution;
}

ObstaclesDetector::~ObstaclesDetector()
{
	lidar->disconnect();
}

ObstaclesDetector.h

#include "GBLIDAR.h"

namespace GBCode {
	class ObstaclesDetector
	{
		public:
			enum DetectionResult {
				kFound,
				kNotFound,
				kError
			};
			/*
				@param distanceThreshlod:	The maximal distance (in mm) before we define something as an obstacle
				@param resolution:			The steps between angle measurements
			*/
			ObstaclesDetector(double distanceThreshold, double resolution = 0.5);
			~ObstaclesDetector();
			DetectionResult obstaclesInRange(double startAngle, double endAngle);
			void setDistanceThreshold(double newThresh);
			void setResolution(double newResolution);

		private:
			double distanceThreshold;
			double resolution;
			DetectionResult obstacleInAngle(double angle);
			GBLIDAR* lidar;

	};
}