source: pacpussensors/trunk/StdDbtPlayerComponents/DbtPlyAlascaManager.cpp@ 150

Last change on this file since 150 was 114, checked in by DHERBOMEZ Gérald, 9 years ago
  • add output to Alasca Player component (come from cityvip project)
  • Delete the force rebuild in build_linux.sh script
File size: 7.1 KB
Line 
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 "kernel/InputOutputInterface.h"
22#include "PacpusTools/ShMem.h"
23
24#define CENTIMETER_TO_METER 100
25
26namespace pacpus {
27
28using namespace std;
29
30DECLARE_STATIC_LOGGER("pacpus.base.DbtPlyAlascaManager");
31
32/// Construction de la fabrique de composant DbtPlyAlascaManager
33static ComponentFactory<DbtPlyAlascaManager> sFactory("DbtPlyAlascaManager");
34
35//static const char * kAlaskaMemoryName = "alasca";// deprecated
36
37//////////////////////////////////////////////////////////////////////////
38/// Constructor.
39DbtPlyAlascaManager::DbtPlyAlascaManager(QString name)
40 : DbtPlyFileManager(name)
41{
42 LOG_TRACE("constructor(" << name << ")");
43
44 // mShMem = new ShMem(kAlaskaMemoryName, sizeof(ScanAlascaData)); // deprecated
45}
46
47//////////////////////////////////////////////////////////////////////////
48/// Destructor.
49DbtPlyAlascaManager::~DbtPlyAlascaManager()
50{
51 LOG_TRACE("destructor");
52 //delete mShMem; // deprecated
53}
54
55//////////////////////////////////////////////////////////////////////////
56/// Configure the component.
57ComponentBase::COMPONENT_CONFIGURATION DbtPlyAlascaManager::configureComponent(XmlComponentConfig config)
58{
59 DbtPlyFileManager::configureComponent(config);
60 mDataFilename = config.getProperty("binFile");
61
62 return ComponentBase::CONFIGURED_OK;
63}
64
65void DbtPlyAlascaManager::addInputs()
66{
67 // empty: no inputs
68}
69
70void DbtPlyAlascaManager::addOutputs()
71{
72 addOutput<LidarScan, DbtPlyAlascaManager>("scan");
73 addOutput<ScanAlascaData, DbtPlyAlascaManager>("raw");
74}
75
76//////////////////////////////////////////////////////////////////////////
77/// Starts the component.
78void DbtPlyAlascaManager::startActivity()
79{
80 mDataFilename = mEngine->getDataDir() + mDataFilename;
81
82 mDataFile.open(mDataFilename.toLatin1().data(),std::ios_base::in|std::ios_base::binary);
83 if (!mDataFile) {
84 LOG_ERROR("cannot open file '" << mDataFilename << "'");
85 return;
86 }
87 DbtPlyFileManager::startActivity();
88}
89
90//////////////////////////////////////////////////////////////////////////
91/// Stops the component.
92void DbtPlyAlascaManager::stopActivity()
93{
94 DbtPlyFileManager::stopActivity();
95 mDataFile.close();
96}
97
98//////////////////////////////////////////////////////////////////////////
99/// processData
100void DbtPlyAlascaManager::processData(road_time_t t, road_timerange_t tr, void * buffer)
101{
102 if (!buffer) {
103 LOG_DEBUG("no data available: NULL buffer");
104 return;
105 }
106
107 LOG_TRACE("sizeof(AlascaXT) = " << sizeof(AlascaXT));
108 BOOST_ASSERT(24 == sizeof(AlascaXT));
109 AlascaXT * alascaXT = static_cast<AlascaXT *>(buffer);
110
111 // copy the values contained in the dbt file
112 mAlascaData.startAngle = alascaXT->startAngle;
113 mAlascaData.endAngle = alascaXT->endAngle;
114 mAlascaData.nbPoint = alascaXT->nbPoint;
115 mAlascaData.scannertype = alascaXT->scannertype;
116 mAlascaData.timeStart = alascaXT->timeStart;
117 mAlascaData.time = t;
118 mAlascaData.timerange = tr;
119
120 // set the get pointer to the correct place
121 mDataFile.seekg(alascaXT->dataPos);
122
123 // then copy the data contained in the binary file
124 for (size_t i = 0; i < mAlascaData.nbPoint; ++i) {
125 mDataFile.read(reinterpret_cast<char *>(&(mAlascaData.point[i].scannerId)), sizeof(uint8_t));
126 mDataFile.read(reinterpret_cast<char *>(&(mAlascaData.point[i].layerNumber)), sizeof(uint8_t));
127 mDataFile.read(reinterpret_cast<char *>(&(mAlascaData.point[i].echoNumber)), sizeof(uint8_t));
128 mDataFile.read(reinterpret_cast<char *>(&(mAlascaData.point[i].pointStatus)), sizeof(uint8_t));
129 mDataFile.read(reinterpret_cast<char *>(&(mAlascaData.point[i].x)), sizeof(int16_t));
130 mDataFile.read(reinterpret_cast<char *>(&(mAlascaData.point[i].y)), sizeof(int16_t));
131 mDataFile.read(reinterpret_cast<char *>(&(mAlascaData.point[i].z)), sizeof(int16_t));
132 mDataFile.read(reinterpret_cast<char *>(&(mAlascaData.point[i].width)), sizeof(uint16_t));
133 }
134
135 lidarScan = LidarScan(4);
136 int layer;
137
138 lidarScan.nbPoint = mAlascaData.nbPoint;
139 lidarScan.time = mAlascaData.time;
140 lidarScan.timerange = mAlascaData.timerange;
141 lidarScan.scannerId;
142 lidarScan.scannerType = mAlascaData.scannertype;
143 lidarScan.nbLayer = 4;
144
145 for (size_t i = 0; i < mAlascaData.nbPoint; ++i) {
146 LidarPoint point;
147
148 layer = mAlascaData.point[i].layerNumber;
149 point.echo = mAlascaData.point[i].echoNumber;
150
151 point.x = (float)mAlascaData.point[i].x / CENTIMETER_TO_METER;
152 point.y = (float)mAlascaData.point[i].y / CENTIMETER_TO_METER;
153 point.z = (float)mAlascaData.point[i].z / CENTIMETER_TO_METER;
154 point.intensity = mAlascaData.point[i].width;
155 point.d = (sqrt(point.x*point.x+point.y*point.y+point.z*point.z) * CENTIMETER_TO_METER);
156
157 lidarScan.layers[layer].nbPoint++;
158 lidarScan.layers[layer].points.append(point);
159 }
160
161 // verify that the last value is the UTC magic word
162 int32_t utcMagicWord = 0;
163 mDataFile.read(reinterpret_cast<char *>(&(utcMagicWord)), sizeof(utcMagicWord));
164 if (UTC_MAGIC_WORD != utcMagicWord) {
165 LOG_WARN("corrupted data, do not use them!");
166 LOG_DEBUG("wrong magic word: EXPECTED=" << UTC_MAGIC_WORD << ", ACTUAL=" << utcMagicWord);
167 } else {
168 LOG_TRACE("writing scan");
169 //mShMem->write(&mAlascaData, sizeof(ScanAlascaData));
170
171 OutputInterface<LidarScan, DbtPlyAlascaManager> * scanOutput = getTypedOutput<LidarScan, DbtPlyAlascaManager>("scan");
172 if (scanOutput && scanOutput->hasConnection()) {
173 scanOutput->send(lidarScan, road_time(), /*timerange=*/50000); // FIXME: magic numbers?
174 }
175 OutputInterface<ScanAlascaData, DbtPlyAlascaManager> * rawOutput = getTypedOutput<ScanAlascaData, DbtPlyAlascaManager>("raw");
176 if (rawOutput && rawOutput->hasConnection()) {
177 rawOutput->send(mAlascaData, road_time(), /*timerange=*/50000);
178 }
179 }
180
181 if (mVerbose) {
182 cout << "[ALASCA]:\t"
183 << "#points=" << mAlascaData.nbPoint << "\t"
184 << "timeStart=" << mAlascaData.timeStart << endl
185 ;
186 }
187 if (mVerbose >= 2) {
188 cout << "[ALASCA]:\t"
189 << "scannertype=" << (int) mAlascaData.scannertype << "\t"
190 << "startAngle=" << mAlascaData.startAngle << "\t"
191 << "endAngle=" << mAlascaData.endAngle << endl
192 ;
193 }
194}
195
196//////////////////////////////////////////////////////////////////////////
197/// Displays the graphical user interface (GUI)
198void DbtPlyAlascaManager::displayUI()
199{
200 LOG_WARN("GUI not implemented");
201
202 // TODO
203}
204
205} // namespace pacpus
Note: See TracBrowser for help on using the repository browser.