ROME 2 Lab5

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers LIDAR.cpp Source File

LIDAR.cpp

00001 /*
00002  * LIDAR.cpp
00003  * Copyright (c) 2020, ZHAW
00004  * All rights reserved.
00005  */
00006 
00007 #include <cmath>
00008 #include "LIDAR.h"
00009 
00010 using namespace std;
00011 
00012 const float LIDAR::DISTANCE_THRESHOLD = 0.01f;  // threshold for measured distance, given in [m]
00013 const float LIDAR::DEFAULT_DISTANCE = 10.0f;    // default distance > range of sensor, given in [m]
00014 const float LIDAR::M_PI = 3.1415926535897932f;  // the mathematical constant PI
00015 const float LIDAR::DISTANCES[] = {10.0f, 10.0f, 10.0f, 10.0f, 10.0f, 10.0f, 10.0f, 10.0f, 10.0f, 10.0f, 10.0f, 10.0f, 10.0f, 10.0f, 10.0f, 10.0f, 10.0f, 1.702465271f, 1.699141254f, 1.69632544f, 1.692140952f, 1.689068974f, 1.68018005f, 1.676267878f, 1.666183663f, 1.671424841f, 1.66193261f, 1.655635528f, 1.653413439f, 1.653517463f, 1.657246512f, 1.655132925f, 1.650946698f, 1.65257254f, 1.66468045f, 1.23646674f, 1.236336928f, 1.251003597f, 1.353653575f, 1.322500662f, 1.304577326f, 1.299988461f, 1.314887448f, 1.320968206f, 1.320374568f, 1.251579003f, 1.235510016f, 1.233241663f, 1.243382483f, 1.314194811f, 1.318788838f, 1.438384163f, 1.419872177f, 1.368804223f, 1.347354445f, 1.342721118f, 1.354318279f, 1.366872708f, 1.369305298f, 1.383822604f, 1.508895291f, 1.493255504f, 1.475824515f, 1.435599178f, 1.445460826f, 1.462035909f, 1.654100964f, 1.644884494f, 1.707480307f, 1.701130213f, 1.660187941f, 1.634974006f, 1.61723344f, 1.620856564f, 1.798737613f, 1.779742116f, 1.77366344f, 1.77661504f, 1.777926039f, 1.920203375f, 1.935389367f, 2.291142292f, 2.328650253f, 2.363611643f, 2.448420103f, 2.487483266f, 2.57330313f, 2.545476969f, 2.040235771f, 2.028301999f, 2.014f, 1.98730823f, 1.972207393f, 1.955661781f, 1.944761168f, 1.923351242f, 1.909502815f, 1.903193369f, 1.875251983f, 1.874046424f, 1.857301806f, 1.845873235f, 1.837153505f, 1.817614371f, 1.803495495f, 1.796232168f, 1.784177401f, 1.781868963f, 1.775984797f, 1.764001134f, 1.761087448f, 1.753326267f, 1.75371748f, 1.745729933f, 1.742740658f, 1.737636613f, 1.741089889f, 1.735240617f, 1.735295076f, 1.728695462f, 1.720377865f, 1.657877257f, 1.727796574f, 1.734111011f, 1.729579429f, 1.736116356f, 1.743193908f, 1.745805545f, 1.750349965f, 1.750429662f, 1.754628166f, 1.760757223f, 1.766661541f, 1.766717012f, 1.776524697f, 1.161069335f, 1.148512081f, 1.14054373f, 1.135753494f, 1.134139321f, 1.147087617f, 1.168199041f, 1.17903223f, 1.18040205f, 1.183327934f, 1.147416228f, 1.210927331f, 1.217378331f, 1.203945597f, 1.227244067f, 1.237879235f, 0.547902364f, 0.544882556f, 0.548745843f, 0.548314691f, 0.553859188f, 0.558237405f, 0.562782374f, 0.572875205f, 0.577382023f, 0.587456381f, 0.593270596f, 0.595157122f, 10.0f, 10.0f, 10.0f, 10.0f, 10.0f, 10.0f, 10.0f, 10.0f, 10.0f, 10.0f, 10.0f, 10.0f, 1.826932128f, 1.764292776f, 1.757409742f, 1.728041956f, 1.704264064f, 1.688f, 1.679250428f, 1.66501051f, 1.644250589f, 1.634979205f, 1.626211548f, 1.608795823f, 1.589880499f, 1.58137788f, 1.575325998f, 1.560708173f, 1.550516043f, 1.54374253f, 1.532342651f, 1.520213472f, 1.512674783f, 1.507533416f, 1.497487229f, 1.494217186f, 1.48919072f, 1.232154617f, 1.358305194f, 1.313616382f, 1.260025397f, 1.133025154f, 1.117565658f, 1.100202254f, 1.100181803f, 1.169824773f, 1.168748476f, 1.164819729f, 1.220040983f, 1.203097669f, 1.200735191f, 1.189294329f, 1.186247866f, 1.195532517f, 1.229603188f, 1.472497538f, 1.474618595f, 1.479985473f, 1.480043918f, 1.484135439f, 1.490767923f, 1.496454476f, 1.501894803f, 1.511313667f, 1.516939682f, 1.525908582f, 1.535359893f, 1.544518695f, 1.549271119f, 1.555904881f, 1.575204114f, 1.580785248f, 0.959320593f, 0.920540059f, 0.905058009f, 0.900680298f, 0.898481497f, 0.902731965f, 0.918059911f, 1.718295085f, 1.721370675f, 1.735719159f, 1.763130455f, 1.775434595f, 1.817423726f, 1.836723441f, 1.847665825f, 1.884798663f, 1.897461462f, 1.83701742f, 1.811276898f, 1.759177649f, 1.740189932f, 1.702600364f, 1.687345845f, 1.637890411f, 1.604450373f, 1.589151031f, 1.55510289f, 1.544042098f, 1.516327801f, 1.502226681f, 1.479634076f, 1.483579792f, 1.518056982f, 1.568964308f, 1.602244675f, 1.663f, 1.700264685f, 1.771085543f, 1.812491379f, 1.888618543f, 1.937385093f, 2.032088827f, 2.089617429f, 2.205471605f, 2.275026373f, 2.345580738f, 2.499928999f, 2.581471286f, 2.784285366f, 2.885689519f, 2.858447306f, 2.850508025f, 2.832713364f, 2.828422882f, 2.812354352f, 2.808188206f, 2.789411407f, 2.789161343f, 2.77675368f, 2.765194568f, 1.967565247f, 1.958f, 2.397585661f, 2.75211991f, 2.743884108f, 2.743676366f, 2.746035688f, 2.735854528f, 10.0f, 10.0f, 10.0f, 2.76492206f, 2.761207888f, 2.762739582f, 2.766510618f, 2.788146338f, 2.786430153f, 2.801847426f, 2.811054073f, 2.691653024f, 2.664378352f, 2.401709391f, 2.204667775f, 2.12351713f, 2.141373625f, 2.14578121f, 2.165700349f, 2.171425799f, 2.185005721f, 2.197850768f, 2.21938122f, 2.229375025f, 2.23809964f, 2.265003532f, 2.644680132f, 2.54522003f, 2.527676008f, 2.480120158f, 2.52638279f, 2.386449455f, 2.36217802f, 2.291129198f, 2.094351451f, 2.007193314f, 2.009421807f, 2.047382719f, 2.035974951f, 1.865168089f, 1.820468346f, 1.800660157f, 1.80447721f, 1.836480329f, 1.730293906f, 1.678679541f, 1.66250203f, 1.677434052f, 1.719175675f, 1.720679226f, 1.622129465f, 1.618845576f, 1.634181141f, 10.0f, 10.0f, 10.0f, 10.0f};
00016 
00017 /**
00018  * Creates a LIDAR object.
00019  * @param serial a reference to a serial interface to communicate with the laser scanner.
00020  */
00021 LIDAR::LIDAR(RawSerial& serial) : serial(serial) {
00022     
00023     // initialize serial interface
00024     
00025     serial.baud(115200);
00026     serial.format(8, SerialBase::None, 1);
00027     
00028     // initialize local values
00029     
00030     headerCounter = 0;
00031     dataCounter = 0;
00032     
00033     for (unsigned short i = 0; i < 360; i++) distances[i] = DEFAULT_DISTANCE;
00034     
00035     simulation = true;
00036     
00037     // start serial interrupt
00038     
00039     serial.attach(callback(this, &LIDAR::receive), RawSerial::RxIrq);
00040     
00041     // start the continuous operation of the LIDAR
00042     
00043     serial.putc(START_FLAG);
00044     serial.putc(SCAN);
00045 }
00046 
00047 /**
00048  * Stops the lidar and deletes this object.
00049  */
00050 LIDAR::~LIDAR() {
00051     
00052     // stop the LIDAR
00053     
00054     serial.putc(START_FLAG);
00055     serial.putc(STOP);
00056 }
00057 
00058 /**
00059  * Get a list of points of a full 360 degree scan.
00060  * @return a deque vector of 360 point objects.
00061  */
00062 deque<Point> LIDAR::getScan() {
00063     
00064     deque<Point> scan;
00065     
00066     for (unsigned short i = 0; i < 360; i++) {
00067         
00068         if (simulation) {
00069             
00070             // use simulated distances, because LIDAR is not available
00071             
00072             scan.push_back(Point(DISTANCES[i]-0.002f*(rand()%10), (float)i*M_PI/180.0f));
00073             
00074         } else {
00075             
00076             // use latest measurements from actual LIDAR
00077             
00078             scan.push_back(Point(distances[i], (float)i*M_PI/180.0f));
00079         }
00080     }
00081     
00082     return scan;
00083 }
00084 
00085 /**
00086  * Get a list of points which are part of beacons.
00087  * @return a deque vector of points that are beacons.
00088  */
00089 deque<Point> LIDAR::getBeacons() {
00090     
00091     // get a list of all points of a scan
00092     
00093     deque<Point> scan = getScan();
00094     
00095     // create a list of points of beacons
00096     
00097     deque<Point> beacons;
00098     
00099     // bitte implementieren!
00100         for (unsigned short i = 0; i < scan.size(); i++) {
00101          
00102               
00103                  bool beacon = true; // flag to check if this point is possibly a beacon
00104             
00105             
00106         // check distance to other points
00107         
00108         for (unsigned short j = 0; j < scan.size(); j++) {
00109             
00110             float distance1 = scan[i].manhattanDistance(scan[j]);
00111             float distance2 = scan[i].manhattanDistance(scan[j+1]);
00112             float distance3 = scan[i].manhattanDistance();
00113             
00114             if (distance3 > 3.0f){
00115                 
00116                 beacon = false; // this point cannot be part of a beacon
00117                 
00118             }
00119                 
00120             if ((distance1 > 0.1f) && (distance1 < 0.5f)) {//
00121                 
00122                 beacon = false; // this point cannot be part of a beacon
00123             }
00124             
00125             if ((distance2 > 0.1f)) {
00126 
00127                beacon = false; // this point cannot be part of a beacon
00128             }
00129             
00130         }
00131         
00132         if (beacon) beacons.push_back(scan[i]);
00133     } 
00134 
00135     
00136     return beacons;
00137 }
00138 
00139 /**
00140  * This method is called by the serial interrupt service routine.
00141  * It handles the reception of measurements from the LIDAR.
00142  */
00143 void LIDAR::receive() {
00144     
00145     // read received characters while input buffer is full
00146     
00147     if (serial.readable()) {
00148         
00149         // read single character from serial interface
00150         
00151         char c = serial.getc();
00152         
00153         // add this character to the header or to the data buffer
00154         
00155         if (headerCounter < HEADER_SIZE) {
00156             headerCounter++;
00157         } else {
00158             if (dataCounter < DATA_SIZE) {
00159                 data[dataCounter] = c;
00160                 dataCounter++;
00161             }
00162             if (dataCounter >= DATA_SIZE) {
00163                 
00164                 // data buffer is full, process measurement
00165                 
00166                 char quality = data[0] >> 2;
00167                 short angle = 360-(((unsigned short)data[1] | ((unsigned short)data[2] << 8)) >> 1)/64;
00168                 float distance = (float)((unsigned short)data[3] | ((unsigned short)data[4] << 8))/4000.0f;
00169                 
00170                 if ((quality < QUALITY_THRESHOLD) || (distance < DISTANCE_THRESHOLD)) distance = DEFAULT_DISTANCE;
00171                 
00172                 // store distance in [m] into array of full scan
00173                 
00174                 while (angle < 0) angle += 360;
00175                 while (angle >= 360) angle -= 360;
00176                 distances[angle] = distance;
00177                 
00178                 // reset data counter and simulation flag
00179                 
00180                 dataCounter = 0;
00181                 simulation = false;
00182             }
00183         }
00184     }
00185 }
00186