Commit 81f0a239 authored by Angel Santamaria-Navarro's avatar Angel Santamaria-Navarro
Browse files

Clang-format style: Mozilla

parent a997b16d
......@@ -38,6 +38,6 @@ INSTALL(FILES ../Findlidar_lite.cmake DESTINATION ${CMAKE_ROOT}/Modules/)
INSTALL(FILES ../99-lidar_lite.rules DESTINATION /etc/udev/rules.d/)
# Restart UDEV to account for new rules.d
execute_process(COMMAND sudo service udev restart)
INSTALL(CODE "execute_process(COMMAND sudo service udev restart)")
ADD_SUBDIRECTORY(examples)
#include "lidar_lite.h"
#include "exceptions/lidar_lite_exceptions.h"
int main(int argc, char *argv[])
int
main(int argc, char* argv[])
{
std::cout << "***************************************************" << std::endl;
std::cout << "* LIDAR LITE TEST APPLICATION *" << std::endl;
std::cout << "***************************************************" << std::endl;
std::cout << "***************************************************"
<< std::endl;
std::cout << "* LIDAR LITE TEST APPLICATION *"
<< std::endl;
std::cout << "***************************************************"
<< std::endl;
std::string serial = "A700evSl";
std::string serial = "A700evSl";
CLidarLite* laser_ptr = new CLidarLite(serial,0);
try
{
laser_ptr->open();
// E1: Get 10 measurements with bias correction
std::cout << std::endl << "***** Example 1: Measurements WITH bias correction: *******" << std::endl;
for (int ii = 0; ii < 10; ++ii)
std::cout << " - Range: " << laser_ptr->get_range(true) << " [cm]" << std::endl;
// E2: New configuration mode
std::cout << std::endl << "***** Example 2: Set new configuration mode. **************" << std::endl;
laser_ptr->set_new_config(5);
CLidarLite* laser_ptr = new CLidarLite(serial, 0);
// E3: Test 2: Get 10 measurements without bias correction
std::cout << std::endl << "***** Example 3: Measurements WITHOUT bias correction *****" << std::endl;
for (int ii = 0; ii < 10; ++ii)
std::cout << " - Range: " << laser_ptr->get_range(false) << " [cm]" << std::endl;
try {
laser_ptr->open();
// E4: Get correlation values
std::cout << std::endl << "***** Example 4: Get correlation values. ******************" << std::endl;
int num_meas = 50;
std::cout << " - Number of measurements to analize: " << num_meas << std::endl;
std::vector<int> corr_val = laser_ptr->get_correlation(num_meas);
std::cout << " - Correlation values: [";
for (unsigned int ii = 0; ii < corr_val.size(); ++ii)
{
if (ii < corr_val.size()-1)
std::cout << corr_val.at(ii) << ",";
else
std::cout << corr_val.at(ii);
std::cout << std::flush;
}
std::cout << "]" << std::endl;
laser_ptr->close();
// E1: Get 10 measurements with bias correction
std::cout << std::endl
<< "***** Example 1: Measurements WITH bias correction: *******"
<< std::endl;
for (int ii = 0; ii < 10; ++ii)
std::cout << " - Range: " << laser_ptr->get_range(true) << " [cm]"
<< std::endl;
std::cout << std::endl;
std::cout << "***************************************************" << std::endl;
std::cout << "* LIDAR LITE TEST APPLICATION *" << std::endl;
std::cout << "***************************************************" << std::endl;
// E2: New configuration mode
std::cout << std::endl
<< "***** Example 2: Set new configuration mode. **************"
<< std::endl;
laser_ptr->set_new_config(5);
}catch(CLidarLiteException &e)
{
std::cout << e.what() << std::endl;
}
// E3: Test 2: Get 10 measurements without bias correction
std::cout << std::endl
<< "***** Example 3: Measurements WITHOUT bias correction *****"
<< std::endl;
for (int ii = 0; ii < 10; ++ii)
std::cout << " - Range: " << laser_ptr->get_range(false) << " [cm]"
<< std::endl;
if(laser_ptr != NULL)
{
laser_ptr = NULL;
delete laser_ptr;
}
// E4: Get correlation values
std::cout << std::endl
<< "***** Example 4: Get correlation values. ******************"
<< std::endl;
int num_meas = 50;
std::cout << " - Number of measurements to analize: " << num_meas
<< std::endl;
std::vector<int> corr_val = laser_ptr->get_correlation(num_meas);
std::cout << " - Correlation values: [";
for (unsigned int ii = 0; ii < corr_val.size(); ++ii) {
if (ii < corr_val.size() - 1)
std::cout << corr_val.at(ii) << ",";
else
std::cout << corr_val.at(ii);
std::cout << std::flush;
}
std::cout << "]" << std::endl;
laser_ptr->close();
std::cout << std::endl;
std::cout << "***************************************************"
<< std::endl;
std::cout << "* LIDAR LITE TEST APPLICATION *"
<< std::endl;
std::cout << "***************************************************"
<< std::endl;
} catch (CLidarLiteException& e) {
std::cout << e.what() << std::endl;
}
if (laser_ptr != NULL) {
laser_ptr = NULL;
delete laser_ptr;
}
}
// Copyright (C) 2009-2010 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Copyright (C) 2009-2010 Institut de Robòtica i Informàtica Industrial,
// CSIC-UPC.
// Author Sergi Hernandez (shernand@iri.upc.edu)
// All rights reserved.
//
......@@ -17,20 +18,21 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#include "exceptions.h"
#include <string.h>
#include <stdio.h>
#include <string.h>
const std::string exception_msg="[Exception caught] - ";
const std::string exception_msg = "[Exception caught] - ";
CException::CException(const std::string& where, const std::string& error_msg)
{
this->error_msg=exception_msg;
this->error_msg+=where;
this->error_msg+="\nError: ";
this->error_msg+=error_msg;
this->error_msg = exception_msg;
this->error_msg += where;
this->error_msg += "\nError: ";
this->error_msg += error_msg;
}
const std::string& CException::what(void)
const std::string&
CException::what(void)
{
return this->error_msg;
}
......
// Copyright (C) 2009-2010 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Copyright (C) 2009-2010 Institut de Robòtica i Informàtica Industrial,
// CSIC-UPC.
// Author Sergi Hernandez (shernand@iri.upc.edu)
// All rights reserved.
//
......@@ -23,8 +24,9 @@
#include <sstream>
#include <string>
template <typename T_>
inline std::string tostring (const T_ & src)
template <typename T_>
inline std::string
tostring(const T_& src)
{
std::ostringstream out;
......@@ -32,24 +34,25 @@ inline std::string tostring (const T_ & src)
return out.str();
}
#define _HERE_ (std::string(__PRETTY_FUNCTION__) + " at " + \
std::string(__FILE__) + ":" +tostring(__LINE__))
#define _HERE_ \
(std::string(__PRETTY_FUNCTION__) + " at " + std::string(__FILE__) + ":" + \
tostring(__LINE__))
/**
/**
* \brief Generic exception
*
* This class inherits from the std::exception class to provide generic
*
* This class inherits from the std::exception class to provide generic
* exceptions for for all classes. The error message is provided at
* construction time with all the necessary information about the error
* that has generated the exception. When the exception is caught, the
* what() function may be used to get the error message.
* that has generated the exception. When the exception is caught, the
* what() function may be used to get the error message.
*
* This class should be used as a base class for all other exception
* classes in any program. This way it will be simple to distinguish
* classes in any program. This way it will be simple to distinguish
* between system exceptions and application exceptions.
*
* When throwing exceptions, it is important to free all allocated
* resources and also to set all the class attributes and variables to a
* When throwing exceptions, it is important to free all allocated
* resources and also to set all the class attributes and variables to a
* known state. Otherwise the behavior of the program after an exception
* is thrown could be unpredictible.
*
......@@ -69,62 +72,63 @@ inline std::string tostring (const T_ & src)
* of the original one.
*
* This code will catch any exception of this class or any inherited class.
* Several catch statements can be used to catch exceptions of different
* Several catch statements can be used to catch exceptions of different
* classes separatelly and handle them in different ways.
*
*/
class CException : public std::exception
{
protected:
/**
* \brief Exception error message.
*
* This attribute is a string which contains the error message with the
* information about the error that has generated the exception. This
* message is allocated and initialized at construction time and can not
* be modified afterwards.
*
* When the exception is caught, the what() function must be used to get
* the contents of this error message.
*/
std::string error_msg;
public:
/**
* \brief Class constructor
*
* The constructor of this class allocates the necessary memory to store
* the error message. In addition to the error message provided to the
* constructor, the name of the function, filename and line where the error
* has ocurred, as well as the text "[Exception caught] - " is pre-appended
* to label the message as an exception.
*
* \param where a null terminated string with the information about the name
* of the function, the source code filename and the line where
* the exception was generated. This string must be generated
* by the _HERE_ macro.
*
* \param error_msg a null terminated string that contains the error message.
* This string may have any valid character and there is no
* limit on its length.
*/
CException(const std::string& where, const std::string& error_msg);
/**
* \brief Function to get the error message
*
* This function is used to get the error message with the information about
* the error that has generated the exception.
*
* \return the error message as a constant string to prevent external
* modifications of the private attributes of the class.
*/
virtual const std::string& what(void);
/**
* \brief Class destructor
*
* This destructor just frees all the memory allocated to store the error
* message.
*/
virtual ~CException() throw();
protected:
/**
* \brief Exception error message.
*
* This attribute is a string which contains the error message with the
* information about the error that has generated the exception. This
* message is allocated and initialized at construction time and can not
* be modified afterwards.
*
* When the exception is caught, the what() function must be used to get
* the contents of this error message.
*/
std::string error_msg;
public:
/**
* \brief Class constructor
*
* The constructor of this class allocates the necessary memory to store
* the error message. In addition to the error message provided to the
* constructor, the name of the function, filename and line where the error
* has ocurred, as well as the text "[Exception caught] - " is pre-appended
* to label the message as an exception.
*
* \param where a null terminated string with the information about the name
* of the function, the source code filename and the line where
* the exception was generated. This string must be generated
* by the _HERE_ macro.
*
* \param error_msg a null terminated string that contains the error message.
* This string may have any valid character and there is no
* limit on its length.
*/
CException(const std::string& where, const std::string& error_msg);
/**
* \brief Function to get the error message
*
* This function is used to get the error message with the information about
* the error that has generated the exception.
*
* \return the error message as a constant string to prevent external
* modifications of the private attributes of the class.
*/
virtual const std::string& what(void);
/**
* \brief Class destructor
*
* This destructor just frees all the memory allocated to store the error
* message.
*/
virtual ~CException() throw();
};
#endif
#include "lidar_lite_exceptions.h"
#include <sstream>
#include <string.h>
#include <stdio.h>
#include <string.h>
const std::string lidar_lite_error_message="LidarLite - ";
const std::string lidar_lite_error_message = "LidarLite - ";
CLidarLiteException::CLidarLiteException(const std::string& where,const std::string &error_msg):CException(where,lidar_lite_error_message)
CLidarLiteException::CLidarLiteException(const std::string& where,
const std::string& error_msg)
: CException(where, lidar_lite_error_message)
{
std::stringstream text;
this->error_msg+=error_msg;
this->error_msg += error_msg;
}
......@@ -5,19 +5,19 @@
/**
* \brief Lidar Lite exception
*
* This class is used to report general errors of the Lidar Lite devices. These
*
* This class is used to report general errors of the Lidar Lite devices. These
* errors are normally caused by invalid parameters passed to the functions.
* In this case the error message is used to identify which error has ocuurred.
*
*/
class CLidarLiteException : public CException
{
public:
/**
* \brief
*/
CLidarLiteException(const std::string& where,const std::string &error_msg);
public:
/**
* \brief
*/
CLidarLiteException(const std::string& where, const std::string& error_msg);
};
#endif
......@@ -4,61 +4,64 @@
#include <ctime.h>
//******************************************************************************
// PUBLIC API
// PUBLIC API
//******************************************************************************
// The device has a 7-bit slave address with a default value of 0x62.
// Fill in new address here if changed.
// Fill in new address here if changed.
// See operating manual for instructions.
unsigned char DEVICE_ID = 0x62;
CLidarLite::CLidarLite(const std::string &serial, const int config)
CLidarLite::CLidarLite(const std::string& serial, const int config)
{
this->serial_ = serial;
this->config_ = config;
this->status_ = IDLE;
this->max_unbiases_meas_ = 0;
// On power-up or reset, the device performs a self-test sequence and initializes
// all registers with default values. After roughly 22 ms distance measurements
// On power-up or reset, the device performs a self-test sequence and
// initializes
// all registers with default values. After roughly 22 ms distance
// measurements
// can be taken with the I2C interface or the Mode Control Pin.
usleep(22000);
}
CLidarLite::~CLidarLite(void)
{
if (this->status_ == RUNNING)
{
if (this->status_ == RUNNING) {
reset();
close();
}
}
void CLidarLite::open(void)
void
CLidarLite::open(void)
{
if (this->status_ != IDLE)
throw CLidarLiteException(_HERE_, "[CLidarLite]: Device cannot be OPENED because it is already running.");
throw CLidarLiteException(
_HERE_,
"[CLidarLite]: Device cannot be OPENED because it is already running.");
try
{
this->adapter_ = new CUSBI2C("adapter");
try {
this->adapter_ = new CUSBI2C("adapter");
// open serial port
std::cout << "[CLidarLite]: Opening Lidar Lite. " << std::endl;
// open serial port
std::cout << "[CLidarLite]: Opening Lidar Lite. " << std::endl;
this->adapter_->open(this->serial_);
usleep(5000); // Delay to initialize sensor.
std::cout << " - USB_I2C serial number: " << this->serial_ << std::endl;
std::cout << " - USB_I2C firmware revision: " << (int)this->adapter_->get_revision() << std::endl;
std::cout << " - USB_I2C firmware revision: "
<< (int)this->adapter_->get_revision() << std::endl;
// Set gpio 2 and 3 as i2c
this->adapter_->config_gpio(gpio2,i2c);
this->adapter_->config_gpio(gpio3,i2c);
// Set gpio 2 and 3 as i2c
this->adapter_->config_gpio(gpio2, i2c);
this->adapter_->config_gpio(gpio3, i2c);
}catch(CException &e)
{
} catch (CException& e) {
throw CLidarLiteException(_HERE_, e.what());
}
}
// Configure device
config_dev();
......@@ -67,29 +70,34 @@ void CLidarLite::open(void)
this->status_ = RUNNING;
}
void CLidarLite::close(void)
void
CLidarLite::close(void)
{
if (this->status_ != RUNNING)
throw CLidarLiteException(_HERE_, "Device cannot be CLOSED because it is not running.");
throw CLidarLiteException(
_HERE_, "Device cannot be CLOSED because it is not running.");
if(this->adapter_ != NULL)
{
if (this->adapter_ != NULL) {
this->adapter_ = NULL;
delete this->adapter_;
delete this->adapter_;
}
}
void CLidarLite::reset(void)
void
CLidarLite::reset(void)
{
if (this->status_ != RUNNING)
throw CLidarLiteException(_HERE_, "Device cannot be RESET because it is not running.");
throw CLidarLiteException(
_HERE_, "Device cannot be RESET because it is not running.");
// reset operation
write(ACQ_COMMAND,0x00);
// reset operation
write(ACQ_COMMAND, 0x00);
std::cout << "[CLidarLite]: Device RESET done." << std::endl;
// On power-up or reset, the device performs a self-test sequence and initializes
// all registers with default values. After roughly 22 ms distance measurements
// On power-up or reset, the device performs a self-test sequence and
// initializes
// all registers with default values. After roughly 22 ms distance
// measurements
// can be taken with the I2C interface or the Mode Control Pin.
usleep(22000);
......@@ -97,82 +105,91 @@ void CLidarLite::reset(void)
config_dev();
}
void CLidarLite::set_new_config(const int config)
void
CLidarLite::set_new_config(const int config)
{
this->config_ = config;
// Configure device
config_dev();
}
int CLidarLite::get_range(bool biasCorrection)
int
CLidarLite::get_range(bool biasCorrection)
{
if (this->status_ != RUNNING)
throw CLidarLiteException(_HERE_, "RANGE MEASUREMENT cannot be obtained because the device is not running.");
throw CLidarLiteException(_HERE_, "RANGE MEASUREMENT cannot be obtained "
"because the device is not running.");
// Initiate an aquisition.
if(biasCorrection || this->max_unbiases_meas_ > 99)
{
if (biasCorrection || this->max_unbiases_meas_ > 99) {
if (this->max_unbiases_meas_ > 99)
std::cout << "\033[1;33m" << "[CLidarLite]: - WARNING : " << "\033[1;37m\033[0m" << "Forcing bias correction. Maximum measurements reached without bias correction (100)." << std::endl;
write(ACQ_COMMAND,0x04);
std::cout << "\033[1;33m"
<< "[CLidarLite]: - WARNING : "
<< "\033[1;37m\033[0m"
<< "Forcing bias correction. Maximum measurements reached "
"without bias correction (100)."
<< std::endl;
write(ACQ_COMMAND, 0x04);
this->max_unbiases_meas_ = 0;
}
else
{
write(ACQ_COMMAND,0x03);
} else {
write(ACQ_COMMAND, 0x03);
++this->max_unbiases_meas_;
}
// Read two bytes from register 0x8f and save
int range = NAN;
range = read(FULL_DELAY_HIGH,2);
range = read(FULL_DELAY_HIGH, 2);
return range;
}
std::vector<int> CLidarLite::get_correlation(const int numberOfReadings)
std::vector<int>
CLidarLite::get_correlation(const int numberOfReadings)
{
if (this->status_ != RUNNING)
throw CLidarLiteException(_HERE_, "CORRELATION between measurements cannot be obtained because the device is not running.");
throw CLidarLiteException(_HERE_, "CORRELATION between measurements cannot "
"be obtained because the device is not "
"running.");
if (numberOfReadings < 2 || numberOfReadings > 1024)
throw CLidarLiteException(_HERE_, "CORRELATION between measurements cannot be obtained. Incorrect selecetd NUMBER OF READINGS, which must be between [2~1024].");
throw CLidarLiteException(
_HERE_, "CORRELATION between measurements cannot be obtained. Incorrect "
"selecetd NUMBER OF READINGS, which must be between [2~1024].");
// Object to return
std::vector<int> correlationValues;
correlationValues.resize(numberOfReadings);
// Select memory bank
write(ACQ_SETTINGS,0xc0);
write(ACQ_SETTINGS, 0xc0);
// Test mode enable
write(COMMAND, 0x07);
for(int ii = 0; ii<numberOfReadings; ii++)
{
for (int ii = 0; ii < numberOfReadings; ii++) {
// Read two bytes from 0xd2
unsigned char addr = addr_to_read_2bytes(CORR_DATA); // See header file for a description of this functionality.
unsigned char addr = addr_to_read_2bytes(
CORR_DATA); // See header file for a description of this functionality.
unsigned char data[2];
try {
this->adapter_->write(DEVICE_ID,&addr,1);
this->adapter_->read(DEVICE_ID,data,2);