A class to interface with the RPLidar Sensor

GBLIDAR.cpp

#include "GBLIDAR.h"

using namespace GBCode;

GBLIDAR::GBLIDAR(const char * com_path, _u32 com_baudrate)
{
	drv = RPlidarDriver::CreateDriver(RPlidarDriver::DRIVER_TYPE_SERIALPORT);

	// make connection...
	if (IS_FAIL(drv->connect(com_path, com_baudrate))) {
		sensorConnected = false;
		return;
	}

	// check health...
	if (!checkSensorHealth()) {
		sensorConnected = false;
		return;
	}

	sensorConnected = true;
}

/*
	Returns A Tuple with two members:
		- A pointer to an array of measurements from the RPLidar sensor (type: rplidar_response_measurement_node_t)
		- The amount of items in the array (type: size_t)
*/
std::tuple<rplidar_response_measurement_node_t*, size_t> GBLIDAR::GetMeasurements() {
	rplidar_response_measurement_node_t nodes[360 * 2];
	size_t   count = _countof(nodes);
	std::tuple<rplidar_response_measurement_node_t*, size_t> res(nodes, count);

	// Get the scan data from the sensor
	u_result op_result = drv->grabScanData(nodes, count);

	// If we got the scan data
	if (IS_OK(op_result)) {
		// Put the data in the variables declered above
		drv->ascendScanData(nodes, count);
		this->measurementsCache = nodes;
		this->cacheSize = count;
		return res;
	}
	// TODO: else {...}
}

/*
	Returns the distance to the point at the closest angle to the requested one
*/
double GBLIDAR::GetDistanceForAngle(double angle, bool useCache){
	rplidar_response_measurement_node_t* nodes;
	size_t count;

	// Get measurement and the amount of items in the nodes array (as a pointer)
	if (useCache) {
		nodes = measurementsCache;
		count = cacheSize;
	}
	else {
		std::tie(nodes, count) = GetMeasurements();
	}

	// Store the closest measure to the requested angle
	rplidar_response_measurement_node_t measure;
	double closestAngle = 720.0;

	// Iterate over all the measurements and find the most suitable
	for (int pos = 0; pos < (int)count; ++pos) {
		if (fabs(angle - rplidarToAngle(nodes[pos].angle_q6_checkbit)) < fabs(closestAngle - angle)) {
			closestAngle = rplidarToAngle(nodes[pos].angle_q6_checkbit);
			measure = nodes[pos];
		}
	}
	// Return the distance
	return rplidarToDistance(measure.distance_q2);
}

/*
	Check the health of the sensor
*/
bool GBLIDAR::checkSensorHealth() {
	u_result     op_result;
	rplidar_response_device_health_t healthinfo;

	// Perform the health check
	op_result = drv->getHealth(healthinfo);

	// the macro IS_OK is the preperred way to judge whether the operation is succeed.
	if (IS_OK(op_result)) {
		// If the health status reports an error
		return healthinfo.status == RPLIDAR_STATUS_ERROR;

	}
	// If the sensor's health status cannot be retrived
	else {
		return false;
	}
}

/*
	Reset the RPLidar sensor
*/
bool GBLIDAR::resetSensor() {
	u_result op_result = drv->reset();
	return IS_OK(op_result);
}

/*
	Start scanning with the sensor
*/
void GBLIDAR::startScan() {
	drv->startScan();
}

/*
	Disconnect from the RPLidar driver
*/
void GBLIDAR::disconnect() {
	RPlidarDriver::DisposeDriver(drv);
	sensorConnected = false;
}

/*
	Check whether the sensor is connected or not
*/
bool GBLIDAR::isConnected() {
	return sensorConnected;
}

GBLIDAR::~GBLIDAR()
{
	if (sensorConnected)
		disconnect();
}

GBLIDAR.h

#include <tuple>
#include <math.h>

#include "../../../sdk/include/rplidar.h" //RPLIDAR standard sdk, all-in-one header

using namespace rp::standalone::rplidar;

// Convert angle data from the RPLidar to degrees
#define rplidarToAngle(alpha) (double)((alpha >> RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT) / 64.0f)
// Convert distance data from the RPLidar to milimeters
#define rplidarToDistance(distance) (double)(distance / 4.0f)

#ifndef _countof
// Count the number of objects in an array
#define _countof(_Array) (int)(sizeof(_Array) / sizeof(_Array[0]))
#endif

namespace GBCode {
	class GBLIDAR
	{
		public:
			GBLIDAR(const char* com_path = "\\\\.\\com4", _u32 com_baudrate = 115200);
			double GetDistanceForAngle(double angle, bool useCache = false);
			void startScan();
			void disconnect();
			bool isConnected();
			bool checkSensorHealth();
			bool resetSensor();
			~GBLIDAR();

		private:
			bool sensorConnected = false;
			std::tuple<rplidar_response_measurement_node_t*, size_t> GetMeasurements();
			rplidar_response_measurement_node_t* measurementsCache;
			size_t cacheSize;
			RPlidarDriver* drv;
	};
}