[89] | 1 | // *********************************************************************
|
---|
| 2 | // created: 2007/11/13 - 16:49
|
---|
| 3 | // filename: DbtPlyAlascaManager.cpp
|
---|
| 4 | //
|
---|
| 5 | // author: Gerald Dherbomez
|
---|
| 6 | // Copyright Heudiasyc UMR UTC/CNRS 6599
|
---|
| 7 | //
|
---|
| 8 | // version: $Id: DbtPlyAlascaManager.cpp 1199 2012-08-01 08:51:24Z kurdejma $
|
---|
| 9 | //
|
---|
| 10 | // purpose:
|
---|
| 11 | // *********************************************************************
|
---|
| 12 |
|
---|
| 13 | #include "DbtPlyAlascaManager.h"
|
---|
| 14 |
|
---|
| 15 | //#include <boost/assert.hpp>
|
---|
| 16 | #include <iostream>
|
---|
| 17 | #include <string>
|
---|
| 18 | #include <math.h>
|
---|
| 19 |
|
---|
| 20 | #include "kernel/Log.h"
|
---|
| 21 | #include "PacpusTools/ShMem.h"
|
---|
| 22 | #include "Pacpus/kernel/inputOutputInterface.h"
|
---|
| 23 |
|
---|
| 24 | #define CENTIMETER_TO_METER 100
|
---|
| 25 |
|
---|
| 26 | namespace pacpus {
|
---|
| 27 |
|
---|
| 28 | using namespace std;
|
---|
| 29 |
|
---|
| 30 | DECLARE_STATIC_LOGGER("pacpus.base.DbtPlyAlascaManager");
|
---|
| 31 |
|
---|
| 32 | /// Construction de la fabrique de composant DbtPlyAlascaManager
|
---|
| 33 | static ComponentFactory<DbtPlyAlascaManager> sFactory("DbtPlyAlascaManager");
|
---|
| 34 |
|
---|
| 35 | static const char * kAlaskaMemoryName = "alasca";
|
---|
| 36 |
|
---|
| 37 | void DbtPlyAlascaManager::addInputOutput()
|
---|
| 38 | {
|
---|
| 39 | output.insert("scan",new OutputInterface<LidarScan,DbtPlyAlascaManager> ("scan",this));
|
---|
| 40 | output.insert("raw",new OutputInterface<ScanAlascaData,DbtPlyAlascaManager> ("raw",this));
|
---|
| 41 | }
|
---|
| 42 |
|
---|
| 43 | //////////////////////////////////////////////////////////////////////////
|
---|
| 44 | /// Constructor.
|
---|
| 45 | DbtPlyAlascaManager::DbtPlyAlascaManager(QString name)
|
---|
| 46 | : DbtPlyFileManager(name)
|
---|
| 47 | {
|
---|
| 48 | LOG_TRACE("constructor(" << name << ")");
|
---|
| 49 |
|
---|
| 50 | mShMem = new ShMem(kAlaskaMemoryName, sizeof(ScanAlascaData));
|
---|
| 51 |
|
---|
| 52 | addInputOutput();
|
---|
| 53 | }
|
---|
| 54 |
|
---|
| 55 | //////////////////////////////////////////////////////////////////////////
|
---|
| 56 | /// Destructor.
|
---|
| 57 | DbtPlyAlascaManager::~DbtPlyAlascaManager()
|
---|
| 58 | {
|
---|
| 59 | LOG_TRACE("destructor");
|
---|
| 60 | delete mShMem;
|
---|
| 61 | }
|
---|
| 62 |
|
---|
| 63 | //////////////////////////////////////////////////////////////////////////
|
---|
| 64 | /// Configure the component.
|
---|
| 65 | ComponentBase::COMPONENT_CONFIGURATION DbtPlyAlascaManager::configureComponent(XmlComponentConfig config)
|
---|
| 66 | {
|
---|
| 67 | DbtPlyFileManager::configureComponent(config);
|
---|
| 68 | mDataFilename = param.getProperty("binFile");
|
---|
| 69 |
|
---|
| 70 | return ComponentBase::CONFIGURED_OK;
|
---|
| 71 | }
|
---|
| 72 |
|
---|
| 73 | //////////////////////////////////////////////////////////////////////////
|
---|
| 74 | /// Starts the component.
|
---|
| 75 | void DbtPlyAlascaManager::startActivity()
|
---|
| 76 | {
|
---|
| 77 | mDataFilename = mEngine->getDataDir() + mDataFilename;
|
---|
| 78 |
|
---|
| 79 | mDataFile.open(mDataFilename.toLatin1().data(),std::ios_base::in|std::ios_base::binary);
|
---|
| 80 | if (!mDataFile) {
|
---|
| 81 | LOG_ERROR("cannot open file '" << mDataFilename << "'");
|
---|
| 82 | return;
|
---|
| 83 | }
|
---|
| 84 | DbtPlyFileManager::startActivity();
|
---|
| 85 | }
|
---|
| 86 |
|
---|
| 87 | //////////////////////////////////////////////////////////////////////////
|
---|
| 88 | /// Stops the component.
|
---|
| 89 | void DbtPlyAlascaManager::stopActivity()
|
---|
| 90 | {
|
---|
| 91 | DbtPlyFileManager::stopActivity();
|
---|
| 92 | mDataFile.close();
|
---|
| 93 | }
|
---|
| 94 |
|
---|
| 95 | //////////////////////////////////////////////////////////////////////////
|
---|
| 96 | /// processData
|
---|
| 97 | void DbtPlyAlascaManager::processData(road_time_t t, road_timerange_t tr, void * buffer)
|
---|
| 98 | {
|
---|
| 99 | if (!buffer) {
|
---|
| 100 | LOG_DEBUG("no data available: NULL buffer");
|
---|
| 101 | return;
|
---|
| 102 | }
|
---|
| 103 |
|
---|
| 104 | LOG_TRACE("sizeof(AlascaXT) = " << sizeof(AlascaXT));
|
---|
| 105 | Q_ASSERT(24 == sizeof(AlascaXT));
|
---|
| 106 | AlascaXT * alascaXT = static_cast<AlascaXT *>(buffer);
|
---|
| 107 |
|
---|
| 108 | // copy the values contained in the dbt file
|
---|
| 109 | mAlascaData.startAngle = alascaXT->startAngle;
|
---|
| 110 | mAlascaData.endAngle = alascaXT->endAngle;
|
---|
| 111 | mAlascaData.nbPoint = alascaXT->nbPoint;
|
---|
| 112 | mAlascaData.scannertype = alascaXT->scannertype;
|
---|
| 113 | mAlascaData.timeStart = alascaXT->timeStart;
|
---|
| 114 | mAlascaData.time = t;
|
---|
| 115 | mAlascaData.timerange = tr;
|
---|
| 116 |
|
---|
| 117 | // set the get pointer to the correct place
|
---|
| 118 | mDataFile.seekg(alascaXT->dataPos);
|
---|
| 119 |
|
---|
| 120 | // then copy the data contained in the binary file
|
---|
| 121 | for (size_t i = 0; i < mAlascaData.nbPoint; ++i) {
|
---|
| 122 | mDataFile.read(reinterpret_cast<char *>(&(mAlascaData.point[i].scannerId)), sizeof(uint8_t));
|
---|
| 123 | mDataFile.read(reinterpret_cast<char *>(&(mAlascaData.point[i].layerNumber)), sizeof(uint8_t));
|
---|
| 124 | mDataFile.read(reinterpret_cast<char *>(&(mAlascaData.point[i].echoNumber)), sizeof(uint8_t));
|
---|
| 125 | mDataFile.read(reinterpret_cast<char *>(&(mAlascaData.point[i].pointStatus)), sizeof(uint8_t));
|
---|
| 126 | mDataFile.read(reinterpret_cast<char *>(&(mAlascaData.point[i].x)), sizeof(int16_t));
|
---|
| 127 | mDataFile.read(reinterpret_cast<char *>(&(mAlascaData.point[i].y)), sizeof(int16_t));
|
---|
| 128 | mDataFile.read(reinterpret_cast<char *>(&(mAlascaData.point[i].z)), sizeof(int16_t));
|
---|
| 129 | mDataFile.read(reinterpret_cast<char *>(&(mAlascaData.point[i].width)), sizeof(uint16_t));
|
---|
| 130 | }
|
---|
| 131 |
|
---|
| 132 | lidarScan = LidarScan(4);
|
---|
| 133 | int layer;
|
---|
| 134 |
|
---|
| 135 | lidarScan.nbPoint = mAlascaData.nbPoint;
|
---|
| 136 | lidarScan.time = mAlascaData.time;
|
---|
| 137 | lidarScan.timerange = mAlascaData.timerange;
|
---|
| 138 | lidarScan.scannerId;
|
---|
| 139 | lidarScan.scannerType = mAlascaData.scannertype;
|
---|
| 140 | lidarScan.nbLayer = 4;
|
---|
| 141 |
|
---|
| 142 | for (int i = 0; i < mAlascaData.nbPoint; ++i) {
|
---|
| 143 | LidarPoint point;
|
---|
| 144 |
|
---|
| 145 | layer = mAlascaData.point[i].layerNumber;
|
---|
| 146 | point.echo = mAlascaData.point[i].echoNumber;
|
---|
| 147 |
|
---|
| 148 | point.x = (float)mAlascaData.point[i].x / CENTIMETER_TO_METER;
|
---|
| 149 | point.y = (float)mAlascaData.point[i].y / CENTIMETER_TO_METER;
|
---|
| 150 | point.z = (float)mAlascaData.point[i].z / CENTIMETER_TO_METER;
|
---|
| 151 | point.intensity = mAlascaData.point[i].width;
|
---|
| 152 | point.d = (sqrt(point.x*point.x+point.y*point.y+point.z*point.z) * CENTIMETER_TO_METER);
|
---|
| 153 |
|
---|
| 154 | lidarScan.layers[layer].nbPoint++;
|
---|
| 155 | lidarScan.layers[layer].points.append(point);
|
---|
| 156 | }
|
---|
| 157 |
|
---|
| 158 | // verify that the last value is the UTC magic word
|
---|
| 159 | int32_t utcMagicWord = 0;
|
---|
| 160 | mDataFile.read(reinterpret_cast<char *>(&(utcMagicWord)), sizeof(utcMagicWord));
|
---|
| 161 | if (UTC_MAGIC_WORD != utcMagicWord) {
|
---|
| 162 | LOG_WARN("corrupted data, do not use them!");
|
---|
| 163 | LOG_DEBUG("wrong magic word: EXPECTED=" << UTC_MAGIC_WORD << ", ACTUAL=" << utcMagicWord);
|
---|
| 164 | } else {
|
---|
| 165 | LOG_TRACE("writing scan");
|
---|
| 166 | mShMem->write(&mAlascaData, sizeof(ScanAlascaData));
|
---|
| 167 |
|
---|
| 168 | OutputInterface<LidarScan,DbtPlyAlascaManager> * out = static_cast<OutputInterface<LidarScan,DbtPlyAlascaManager> *> (output.value("scan"));
|
---|
| 169 | OutputInterface<ScanAlascaData,DbtPlyAlascaManager> * outraw = static_cast<OutputInterface<ScanAlascaData,DbtPlyAlascaManager> *> (output.value("raw"));
|
---|
| 170 | out->send(lidarScan);
|
---|
| 171 | outraw->send(mAlascaData);
|
---|
| 172 | }
|
---|
| 173 |
|
---|
| 174 | if (mVerbose) {
|
---|
| 175 | cout << "[ALASCA]:\t"
|
---|
| 176 | << "#points=" << mAlascaData.nbPoint << "\t"
|
---|
| 177 | << "timeStart=" << mAlascaData.timeStart << endl
|
---|
| 178 | ;
|
---|
| 179 | }
|
---|
| 180 | if (mVerbose >= 2) {
|
---|
| 181 | cout << "[ALASCA]:\t"
|
---|
| 182 | << "scannertype=" << (int) mAlascaData.scannertype << "\t"
|
---|
| 183 | << "startAngle=" << mAlascaData.startAngle << "\t"
|
---|
| 184 | << "endAngle=" << mAlascaData.endAngle << endl
|
---|
| 185 | ;
|
---|
| 186 | }
|
---|
| 187 | }
|
---|
| 188 |
|
---|
| 189 | //////////////////////////////////////////////////////////////////////////
|
---|
| 190 | /// Displays the graphical user interface (GUI)
|
---|
| 191 | void DbtPlyAlascaManager::displayUI()
|
---|
| 192 | {
|
---|
| 193 | LOG_WARN("GUI not implemented");
|
---|
| 194 |
|
---|
| 195 | // TODO
|
---|
| 196 | }
|
---|
| 197 |
|
---|
| 198 | } // namespace pacpus
|
---|