1 | /*********************************************************************
|
---|
2 | // created: 2007/11/07 - 12:08
|
---|
3 | // filename: AlascaComponent.cpp
|
---|
4 | //
|
---|
5 | // author: Gerald Dherbomez & Sergio Rodriguez
|
---|
6 | // Copyright Heudiasyc UMR UTC/CNRS 6599
|
---|
7 | //
|
---|
8 | // version: $Id: $
|
---|
9 | //
|
---|
10 | // purpose: The acquisition component of the Ibeo Alasca sensor
|
---|
11 | //
|
---|
12 | *********************************************************************/
|
---|
13 |
|
---|
14 | #include "AlascaComponent.h"
|
---|
15 |
|
---|
16 | #include "AlascaDataGenerator.h"
|
---|
17 | #include "AlascaSocket.h"
|
---|
18 | #include "kernel/ComponentFactory.h"
|
---|
19 | #include "kernel/DbiteException.h"
|
---|
20 | #include "kernel/DbiteFileTypes.h"
|
---|
21 | #include "kernel/Log.h"
|
---|
22 | #include "PacpusTools/ShMem.h"
|
---|
23 |
|
---|
24 | #include <iostream>
|
---|
25 | #include <QtNetwork/QTcpSocket>
|
---|
26 | #include <string>
|
---|
27 | #include "Pacpus/kernel/inputOutputInterface.h"
|
---|
28 |
|
---|
29 | using namespace std;
|
---|
30 |
|
---|
31 | namespace pacpus {
|
---|
32 |
|
---|
33 | DECLARE_STATIC_LOGGER("pacpus.base.AlascaComponent");
|
---|
34 |
|
---|
35 | // Construct the factory
|
---|
36 | static ComponentFactory<AlascaComponent> sFactory("AlascaComponent");
|
---|
37 |
|
---|
38 | static const string kAlaskaMemoryName = "alasca";
|
---|
39 | static const string kAlaskaDbtFileName = "alasca.dbt";
|
---|
40 | static const string kAlaskaUtcFileName = "alasca_data.utc";
|
---|
41 |
|
---|
42 | static const long MagicWord = 0x55544300;
|
---|
43 |
|
---|
44 | static const int ALASCA_OBJECTDATA_TYPE = 1;
|
---|
45 | static const int ALASCA_SCANDATA_TYPE = 15;
|
---|
46 |
|
---|
47 | void AlascaComponent::addInputOutput()
|
---|
48 | {
|
---|
49 | ADD_OUTPUT("scan",AlascaComponent,LidarScan);
|
---|
50 | ADD_OUTPUT("raw",AlascaComponent,ScanAlascaData);
|
---|
51 | //output.insert("scan",new OutputInterface<LidarScan,AlascaComponent> ("scan",this));
|
---|
52 | //output.insert("raw",new OutputInterface<ScanAlascaData,AlascaComponent> ("raw",this));
|
---|
53 | }
|
---|
54 |
|
---|
55 | //////////////////////////////////////////////////////////////////////////
|
---|
56 | /// Constructor.
|
---|
57 | AlascaComponent::AlascaComponent(QString name)
|
---|
58 | : ComponentBase(name)
|
---|
59 | {
|
---|
60 | LOG_TRACE("constructor(" << name << ")");
|
---|
61 | A_socket = new AlascaSocket(this);
|
---|
62 | pendingBytes.time = 0;
|
---|
63 | pendingBytes.previousData = false;
|
---|
64 | recording = false;
|
---|
65 | // default values
|
---|
66 | host_ = "172.17.130.205";
|
---|
67 | port_ = 12000;
|
---|
68 | addInputOutput();
|
---|
69 | }
|
---|
70 |
|
---|
71 | //////////////////////////////////////////////////////////////////////////
|
---|
72 | /// Destructor.
|
---|
73 | AlascaComponent::~AlascaComponent()
|
---|
74 | {
|
---|
75 | LOG_TRACE("destructor");
|
---|
76 | delete A_socket;
|
---|
77 | }
|
---|
78 |
|
---|
79 | //////////////////////////////////////////////////////////////////////////
|
---|
80 | /// Called by the ComponentManager to start the component
|
---|
81 | void AlascaComponent::startActivity()
|
---|
82 | {
|
---|
83 | // uncomment the following line to use the Alasca Data generator and comment the second
|
---|
84 | //generator = new AlascaDataGenerator(this);
|
---|
85 | A_socket->connectToServer(host_, port_);
|
---|
86 | if (recording) {
|
---|
87 | try {
|
---|
88 | dbtFile_.open(kAlaskaDbtFileName, WriteMode, ALASCA_XT, sizeof(AlascaXT));
|
---|
89 | } catch (DbiteException & e) {
|
---|
90 | cerr << "error opening dbt file: " << e.what() << endl;
|
---|
91 | return;
|
---|
92 | }
|
---|
93 |
|
---|
94 | // FIXME: use ofstream
|
---|
95 | // open the file with C function to be sure that it will exist
|
---|
96 | FILE * stream;
|
---|
97 | if (NULL == (stream = fopen(kAlaskaUtcFileName.c_str(), "a+"))) {
|
---|
98 | LOG_FATAL("cannot open file '" << kAlaskaUtcFileName.c_str() << "'");
|
---|
99 | ::exit(-1);
|
---|
100 | } else {
|
---|
101 | fclose(stream);
|
---|
102 | }
|
---|
103 |
|
---|
104 | dataFile_.open(kAlaskaUtcFileName.c_str(), ios_base::out|ios_base::binary|ios_base::app);
|
---|
105 | if (!dataFile_) {
|
---|
106 | LOG_FATAL("cannot open file '" << kAlaskaUtcFileName.c_str() << "'");
|
---|
107 | ::exit(-1);
|
---|
108 | }
|
---|
109 | }
|
---|
110 | shmem_ = new ShMem(kAlaskaMemoryName.c_str(), sizeof(ScanAlascaData));
|
---|
111 | }
|
---|
112 |
|
---|
113 | //////////////////////////////////////////////////////////////////////////
|
---|
114 | /// Called by the ComponentManager to stop the component
|
---|
115 | void AlascaComponent::stopActivity()
|
---|
116 | {
|
---|
117 | A_socket->closeSocket();
|
---|
118 | if (recording) {
|
---|
119 | dbtFile_.close();
|
---|
120 | dataFile_.close();
|
---|
121 | }
|
---|
122 | delete shmem_;
|
---|
123 | // delete generator;
|
---|
124 | }
|
---|
125 |
|
---|
126 | //////////////////////////////////////////////////////////////////////////
|
---|
127 | /// Called by the ComponentManager to pass the XML parameters to the
|
---|
128 | /// component
|
---|
129 | ComponentBase::COMPONENT_CONFIGURATION AlascaComponent::configureComponent(XmlComponentConfig config)
|
---|
130 | {
|
---|
131 | if (param.getProperty("recording") != QString::null)
|
---|
132 | recording = param.getProperty("recording").toInt();
|
---|
133 | if (param.getProperty("alascaIP") != QString::null)
|
---|
134 | host_ = param.getProperty("alascaIP");
|
---|
135 | if (param.getProperty("alascaPort") != QString::null)
|
---|
136 | port_ = param.getProperty("alascaPort").toInt();
|
---|
137 |
|
---|
138 | return ComponentBase::CONFIGURED_OK;
|
---|
139 | }
|
---|
140 |
|
---|
141 | //////////////////////////////////////////////////////////////////////////
|
---|
142 | /// look for the position of the magic word in the packet.
|
---|
143 | /// Return it if found else return -1
|
---|
144 | long AlascaComponent::findMagicWord(const char * message, const unsigned length)
|
---|
145 | {
|
---|
146 | if (length < 4) {
|
---|
147 | return -1;
|
---|
148 | }
|
---|
149 |
|
---|
150 | unsigned long i = 0;
|
---|
151 | unsigned long magicWord=0x00000000;
|
---|
152 | magicWord = ( (*(message+i+3)) & 0x000000FF )
|
---|
153 | + ( ((*(message+i+2))<<8) & 0x0000FF00 )
|
---|
154 | + ( ((*(message+i+1))<<16) & 0x00FF0000 )
|
---|
155 | + ( ((*(message+i))<<24) & 0xFF000000 );
|
---|
156 |
|
---|
157 | while (magicWord !=0xAFFEC0C0) {
|
---|
158 | ++i;
|
---|
159 | magicWord = ( (*(message+i+3)) & 0x000000FF )
|
---|
160 | + ( ((*(message+i+2))<<8) & 0x0000FF00 )
|
---|
161 | + ( ((*(message+i+1))<<16) & 0x00FF0000 )
|
---|
162 | + ( ((*(message+i))<<24) & 0xFF000000 );
|
---|
163 |
|
---|
164 | if (i == length) {
|
---|
165 | return -1;
|
---|
166 | }
|
---|
167 | continue;
|
---|
168 | }
|
---|
169 | return i;
|
---|
170 | }
|
---|
171 |
|
---|
172 | //////////////////////////////////////////////////////////////////////////
|
---|
173 | /// Return the size of the message contained in the packet
|
---|
174 | /// You have to provide the position of the magic word 0xAFFEC0C0 in the packet
|
---|
175 | long AlascaComponent::getMessageSize(const char * message, const unsigned length, const long magicWordIndex)
|
---|
176 | {
|
---|
177 | // we need at least 8 bytes
|
---|
178 | if (length < 8) {
|
---|
179 | return 0;
|
---|
180 | }
|
---|
181 | return ((*(message+magicWordIndex+7))&0x000000FF)
|
---|
182 | + ((*(message+magicWordIndex+6)<<8)&0x0000FF00)
|
---|
183 | + ((*(message+magicWordIndex+5)<<16)&0x00FF0000)
|
---|
184 | + ((*(message+magicWordIndex+4)<<24)&0xFF000000)
|
---|
185 | + 16;
|
---|
186 | }
|
---|
187 |
|
---|
188 | //////////////////////////////////////////////////////////////////////////
|
---|
189 | /// @param size the total size of the message including the 2 headers and the body
|
---|
190 | /// @param length the number of available bytes in the packet
|
---|
191 | bool AlascaComponent::isMessageComplete(const unsigned length, const long size)
|
---|
192 | {
|
---|
193 | if (size <= length) {
|
---|
194 | return true;
|
---|
195 | }
|
---|
196 | return false;
|
---|
197 | }
|
---|
198 |
|
---|
199 | //////////////////////////////////////////////////////////////////////////
|
---|
200 | /// fill the scan header of the message msg
|
---|
201 | /// warning: the body field of the message have to be completed before
|
---|
202 | void AlascaComponent::fillScanHeader( Message &msg )
|
---|
203 | {
|
---|
204 | msg.hScan.version = *(msg.body+16);
|
---|
205 | msg.hScan.scannerType = *(msg.body+17);
|
---|
206 | msg.hScan.ecuId = *(msg.body+18);
|
---|
207 | // byte 19 is a padding byte
|
---|
208 |
|
---|
209 | msg.hScan.timeStamp = ((*(msg.body+23))&0x000000FF)+
|
---|
210 | ((*(msg.body+22)<<8)&0x0000FF00)+
|
---|
211 | ((*(msg.body+21)<<16)&0x00FF0000)+
|
---|
212 | ((*(msg.body+20)<<24)&0xFF000000);
|
---|
213 |
|
---|
214 | msg.hScan.startAngle = ((*(msg.body+25))&0x00FF)+
|
---|
215 | ((*(msg.body+24)<<8)&0xFF00);
|
---|
216 |
|
---|
217 | msg.hScan.endAngle = ((*(msg.body+27))&0x00FF)+
|
---|
218 | ((*(msg.body+26)<<8)&0xFF00);
|
---|
219 |
|
---|
220 | msg.hScan.scanCounter = ((*(msg.body+29))&0x00FF)+
|
---|
221 | ((*(msg.body+28)<<8)&0xFF00);
|
---|
222 |
|
---|
223 | msg.hScan.numPoints = ((*(msg.body+31))&0x00FF)+
|
---|
224 | ((*(msg.body+30)<<8)&0xFF00);
|
---|
225 | }
|
---|
226 |
|
---|
227 | //////////////////////////////////////////////////////////////////////////
|
---|
228 | /// fill the message header of the message msg
|
---|
229 | /// warning: the body field of the message have to be completed before
|
---|
230 | void AlascaComponent::fillMessageHeader(Message &msg)
|
---|
231 | {
|
---|
232 | msg.hMsg.magicWord = ((*(msg.body+3))&0x000000FF) +
|
---|
233 | ((*(msg.body+2)<<8)&0x0000FF00) +
|
---|
234 | ((*(msg.body+1)<<16)&0x00FF0000)+
|
---|
235 | ((*(msg.body)<<24)&0xFF000000);
|
---|
236 |
|
---|
237 | msg.hMsg.size = ((*(msg.body+7))&0x000000FF)+
|
---|
238 | ((*(msg.body+6)<<8)&0x0000FF00)+
|
---|
239 | ((*(msg.body+5)<<16)&0x00FF0000)+
|
---|
240 | ((*(msg.body+4)<<24)&0xFF000000);
|
---|
241 |
|
---|
242 | msg.hMsg.dataType = ((*(msg.body+11))&0x000000FF)+
|
---|
243 | ((*(msg.body+10)<<8)&0x0000FF00)+
|
---|
244 | ((*(msg.body+9)<<16)&0x00FF0000)+
|
---|
245 | ((*(msg.body+8)<<24)&0xFF000000);
|
---|
246 |
|
---|
247 | msg.hMsg.timestamp =((*(msg.body+15))&0x000000FF)+
|
---|
248 | ((*(msg.body+14)<<8)&0x0000FF00)+
|
---|
249 | ((*(msg.body+13)<<16)&0x00FF0000)+
|
---|
250 | ((*(msg.body+12)<<24)&0xFF000000);
|
---|
251 | }
|
---|
252 |
|
---|
253 | //////////////////////////////////////////////////////////////////////////
|
---|
254 | /// this function is called when no complete message has been found
|
---|
255 | /// we set the flag previousData to true for the next processing stage
|
---|
256 | /// and we store the timestamp of the bytes acquisition
|
---|
257 | void AlascaComponent::storePendingBytes(road_time_t time)
|
---|
258 | {
|
---|
259 | if (!pendingBytes.previousData)
|
---|
260 | {
|
---|
261 | pendingBytes.time = time;
|
---|
262 | pendingBytes.previousData = true;
|
---|
263 | }
|
---|
264 | }
|
---|
265 |
|
---|
266 | //////////////////////////////////////////////////////////////////////////
|
---|
267 | /// Analyse the ethernet packet received from the Alasca and try to find a
|
---|
268 | /// complete message (scan data message or object message)
|
---|
269 | /// If a message has been found it is added at the end of the message list
|
---|
270 | /// else the pending bytes are stored to be analyzed by further icoming data
|
---|
271 | //////////////////////////////////////////////////////////////////////////
|
---|
272 | void AlascaComponent:: splitPacket(const char * packet, const int length, road_time_t time)
|
---|
273 | {
|
---|
274 | //unsigned long ptr = 0;
|
---|
275 | long index = 0;
|
---|
276 | long msgSize = 0;
|
---|
277 | bool msgComplete = false;
|
---|
278 |
|
---|
279 | // we are working on the previous not decoded data + the actual incoming packet
|
---|
280 | pendingBytes.data.append(packet,length);
|
---|
281 |
|
---|
282 | while (pendingBytes.data.size() > 0)
|
---|
283 | {
|
---|
284 | // we are looking for the MagicWord
|
---|
285 | index = findMagicWord(pendingBytes.data.c_str() , pendingBytes.data.size() );
|
---|
286 | if (index == -1)
|
---|
287 | {
|
---|
288 | storePendingBytes(time);
|
---|
289 | // exit the while loop
|
---|
290 | break;
|
---|
291 | }
|
---|
292 |
|
---|
293 | // we are looking for the size of the message (message header + scan header + body)
|
---|
294 | msgSize = getMessageSize(pendingBytes.data.c_str() , pendingBytes.data.size() , index );
|
---|
295 | if (msgSize == 0)
|
---|
296 | {
|
---|
297 | storePendingBytes(time);
|
---|
298 | // exit the while loop
|
---|
299 | break;
|
---|
300 | }
|
---|
301 |
|
---|
302 | // we are verifying if the message is complete
|
---|
303 | msgComplete = isMessageComplete( pendingBytes.data.size() , msgSize );
|
---|
304 | if (msgComplete == false)
|
---|
305 | {
|
---|
306 | storePendingBytes(time);
|
---|
307 | // exit the while loop
|
---|
308 | break;
|
---|
309 | }
|
---|
310 |
|
---|
311 | // we have a complete message available that we can add to the list
|
---|
312 | Message msg;
|
---|
313 | // we copy the bytes in the body message
|
---|
314 | memcpy(msg.body, pendingBytes.data.c_str() + index, msgSize);
|
---|
315 |
|
---|
316 | // we set the timestamp of the message
|
---|
317 | if (pendingBytes.previousData)
|
---|
318 | {
|
---|
319 | // the timestamp is the one of the previous packet
|
---|
320 | msg.time = pendingBytes.time;
|
---|
321 | pendingBytes.previousData = false;
|
---|
322 | }
|
---|
323 | else
|
---|
324 | {
|
---|
325 | // the timestamp is the one of the actual received packet
|
---|
326 | msg.time = time;
|
---|
327 | }
|
---|
328 |
|
---|
329 | // we add the message to the list
|
---|
330 | msgList.push_back(msg);
|
---|
331 | // and we suppress the processed bytes of the pending data
|
---|
332 | // problème ici
|
---|
333 | pendingBytes.data.erase(0, msgSize);
|
---|
334 | }
|
---|
335 | }
|
---|
336 |
|
---|
337 | //////////////////////////////////////////////////////////////////////////
|
---|
338 | /// Convert the integer value returned by the sensor in a cartesian coordinate
|
---|
339 | /// return the value in centimeters
|
---|
340 | /// see Manual_ALASCA.pdf p29/30, note 8
|
---|
341 | //////////////////////////////////////////////////////////////////////////
|
---|
342 | short AlascaComponent::din70000toCentimeters(short n)
|
---|
343 | {
|
---|
344 | // remark: short is included between -32768 and 32767 so if n>10000 or n<-10000
|
---|
345 | // the return value must be limited to these values and the max and min values
|
---|
346 | // of n are obtained by these expressions:
|
---|
347 | // 10 * n_max - 90000 <= 32767 so n_max <= 12276
|
---|
348 | // 10 * n_min + 90000 >= -32768 so n_min >= -12276
|
---|
349 | // the return value will be rounded to this value if n exceeds this limit
|
---|
350 | short ret = 0;
|
---|
351 | if (n>=32768)
|
---|
352 | n = n - 65536;
|
---|
353 | if ( (n>10000) && (n<=12276) )
|
---|
354 | ret = 10 * n - 90000;
|
---|
355 | if ( n>12276 )
|
---|
356 | ret = 32767;
|
---|
357 | if ( (n<-10000) && (n>=-12276) )
|
---|
358 | ret = 10 * n + 90000;
|
---|
359 | if (n<-12276)
|
---|
360 | ret = -32768;
|
---|
361 | if ( (n >= -10000) && (n <= 10000) )
|
---|
362 | ret = n;
|
---|
363 | return ret;
|
---|
364 | }
|
---|
365 |
|
---|
366 | //////////////////////////////////////////////////////////////////////////
|
---|
367 | /// Convert the integer value returned by the sensor in a cartesian coordinate
|
---|
368 | /// return the value in meters
|
---|
369 | /// see Manual_ALASCA.pdf p29/30, note 8
|
---|
370 | //////////////////////////////////////////////////////////////////////////
|
---|
371 | float AlascaComponent::din70000toMeters(short n)
|
---|
372 | {
|
---|
373 | float ret = 0.0;
|
---|
374 | if (n>=32768) n = n - 65536;
|
---|
375 | if (n>10000) ret = n*0.1 - 900 ;
|
---|
376 | if (n<-10000) ret = n*0.1 + 900;
|
---|
377 | if ( (n>= -10000) && (n <= 10000) ) ret = n*0.01;
|
---|
378 | return ret;
|
---|
379 | }
|
---|
380 |
|
---|
381 | //////////////////////////////////////////////////////////////////////////
|
---|
382 | /// Process the message
|
---|
383 | /// Fill the 2 headers (message and scan) and update the scan data
|
---|
384 | /// process only ALASCA_SCANDATA_TYPE message
|
---|
385 | unsigned long AlascaComponent::processMessage(Message &msg)
|
---|
386 | {
|
---|
387 |
|
---|
388 | fillMessageHeader(msg);
|
---|
389 |
|
---|
390 | if (ALASCA_SCANDATA_TYPE == msg.hMsg.dataType) {
|
---|
391 | fillScanHeader(msg);
|
---|
392 |
|
---|
393 | // fill the alascaData structure
|
---|
394 | alascaData.nbPoint = msg.hScan.numPoints;
|
---|
395 | alascaData.startAngle = msg.hScan.startAngle;
|
---|
396 | alascaData.endAngle = msg.hScan.endAngle;
|
---|
397 | alascaData.scannertype = msg.hScan.scannerType;
|
---|
398 | alascaData.timeStart = msg.hScan.timeStamp;
|
---|
399 | alascaData.time = msg.time;
|
---|
400 | alascaData.timerange = msg.timerange;
|
---|
401 |
|
---|
402 | int index = 32;
|
---|
403 | short value;
|
---|
404 |
|
---|
405 | for (int i = 0; i < msg.hScan.numPoints; ++i) {
|
---|
406 | alascaData.point[i].scannerId = *(msg.body + index);
|
---|
407 | alascaData.point[i].layerNumber = *(msg.body + index + 1);
|
---|
408 | alascaData.point[i].echoNumber = *(msg.body + index + 2);
|
---|
409 | alascaData.point[i].pointStatus = *(msg.body + index + 3);
|
---|
410 |
|
---|
411 | value = ((*(msg.body + index + 5))&(0x00FF))+
|
---|
412 | ((*(msg.body + index + 4)<<8)&(0xFF00));
|
---|
413 | alascaData.point[i].x = din70000toCentimeters(value);
|
---|
414 | //alascaData.point[i].x = din70000toMeters(value);
|
---|
415 |
|
---|
416 | value = ((*(msg.body + index + 7))&(0x00FF))+
|
---|
417 | ( (*(msg.body + index + 6)<<8)&(0xFF00));
|
---|
418 | alascaData.point[i].y = din70000toCentimeters(value);
|
---|
419 | //alascaData.point[i].y = din70000toMeters(value);
|
---|
420 |
|
---|
421 | value = ((*(msg.body + index + 9))&(0x00FF))+
|
---|
422 | ((*(msg.body + index + 8)<<8)&(0xFF00));
|
---|
423 | alascaData.point[i].z = din70000toCentimeters(value);
|
---|
424 | //alascaData.point[i].z = din70000toMeters(value);
|
---|
425 |
|
---|
426 | alascaData.point[i].width = ((*(msg.body + index + 11))&(0x00FF))+
|
---|
427 | ((*(msg.body + index + 10)<<8)&(0xFF00));
|
---|
428 |
|
---|
429 | index += 12;
|
---|
430 |
|
---|
431 |
|
---|
432 | // update the dbt structure
|
---|
433 | dbtData_.endAngle = alascaData.endAngle;
|
---|
434 | dbtData_.startAngle = alascaData.startAngle;
|
---|
435 | dbtData_.scannertype = alascaData.scannertype;
|
---|
436 | dbtData_.timeStart = alascaData.timeStart;
|
---|
437 | dbtData_.nbPoint = alascaData.nbPoint;
|
---|
438 | }
|
---|
439 |
|
---|
440 | lidarScan = LidarScan(4);
|
---|
441 | int layer;
|
---|
442 |
|
---|
443 | lidarScan.nbPoint = msg.hScan.numPoints;
|
---|
444 | lidarScan.time = msg.time;
|
---|
445 | lidarScan.timerange = msg.timerange;
|
---|
446 | lidarScan.scannerId;
|
---|
447 | lidarScan.scannerType = msg.hScan.scannerType;
|
---|
448 | lidarScan.nbLayer = 4;
|
---|
449 |
|
---|
450 | for (int i = 0; i < msg.hScan.numPoints; ++i) {
|
---|
451 |
|
---|
452 | LidarPoint point;
|
---|
453 |
|
---|
454 | layer = *(msg.body + index + 1);
|
---|
455 | point.echo = *(msg.body + index + 2);
|
---|
456 |
|
---|
457 | value = ((*(msg.body + index + 5))&(0x00FF))+
|
---|
458 | ((*(msg.body + index + 4)<<8)&(0xFF00));
|
---|
459 | point.x = din70000toMeters(value);
|
---|
460 |
|
---|
461 | value = ((*(msg.body + index + 7))&(0x00FF))+
|
---|
462 | ( (*(msg.body + index + 6)<<8)&(0xFF00));
|
---|
463 | point.y = din70000toMeters(value);
|
---|
464 |
|
---|
465 | value = ((*(msg.body + index + 9))&(0x00FF))+
|
---|
466 | ((*(msg.body + index + 8)<<8)&(0xFF00));
|
---|
467 | point.z = din70000toMeters(value);
|
---|
468 |
|
---|
469 | point.intensity = ((*(msg.body + index + 11))&(0x00FF))+
|
---|
470 | ((*(msg.body + index + 10)<<8)&(0xFF00));
|
---|
471 |
|
---|
472 | lidarScan.layers[layer].nbPoint++;
|
---|
473 | lidarScan.layers[layer].points.append(point);
|
---|
474 |
|
---|
475 | index += 12;
|
---|
476 | }
|
---|
477 |
|
---|
478 | }
|
---|
479 |
|
---|
480 | return msg.hMsg.dataType;
|
---|
481 | }
|
---|
482 |
|
---|
483 | //////////////////////////////////////////////////////////////////////////
|
---|
484 | /// write the data on the disk:
|
---|
485 | /// - complete the dbt file with dbtData_
|
---|
486 | /// - complete the binary data file with the alascaData.point
|
---|
487 | void AlascaComponent::writeData()
|
---|
488 | {
|
---|
489 | // get the absolute position of the put pointer
|
---|
490 | dbtData_.dataPos = dataFile_.tellp();
|
---|
491 |
|
---|
492 | // record the data in a dbt file. The dbt data is only the structure AlascaXT
|
---|
493 | // scan data are recording in the alasca_data.utc file
|
---|
494 | try {
|
---|
495 | dbtFile_.writeRecord(alascaData.time, alascaData.timerange, (char *) &dbtData_, sizeof(dbtData_));
|
---|
496 | } catch (DbiteException & e) {
|
---|
497 | cerr << "error writing data: " << e.what() << endl;
|
---|
498 | return;
|
---|
499 | }
|
---|
500 |
|
---|
501 | // record the scan data in a binary file alasca_data.utc with UTC\0 to separate the data
|
---|
502 | for (unsigned int i = 0 ; i < alascaData.nbPoint ; ++i) {
|
---|
503 | dataFile_.write(reinterpret_cast<char*>(&(alascaData.point[i].scannerId)), sizeof(uint8_t));
|
---|
504 | dataFile_.write(reinterpret_cast<char*>(&(alascaData.point[i].layerNumber)), sizeof(uint8_t));
|
---|
505 | dataFile_.write(reinterpret_cast<char*>(&(alascaData.point[i].echoNumber)), sizeof(uint8_t));
|
---|
506 | dataFile_.write(reinterpret_cast<char*>(&(alascaData.point[i].pointStatus)), sizeof(uint8_t));
|
---|
507 | dataFile_.write(reinterpret_cast<char*>(&(alascaData.point[i].x)), sizeof(int16_t));
|
---|
508 | dataFile_.write(reinterpret_cast<char*>(&(alascaData.point[i].y)), sizeof(int16_t));
|
---|
509 | dataFile_.write(reinterpret_cast<char*>(&(alascaData.point[i].z)), sizeof(int16_t));
|
---|
510 | dataFile_.write(reinterpret_cast<char*>(&(alascaData.point[i].width)), sizeof(uint16_t));
|
---|
511 | }
|
---|
512 | // add a magic word to delimit the block of data
|
---|
513 | int32_t utcMagicWord = UTC_MAGIC_WORD;
|
---|
514 | dataFile_.write(reinterpret_cast<char*>(&(utcMagicWord)), sizeof(int32_t));
|
---|
515 | }
|
---|
516 |
|
---|
517 | //////////////////////////////////////////////////////////////////////////
|
---|
518 | /// Event call by the AlascaSocket class when new data has been arrived
|
---|
519 | /// on the network.
|
---|
520 | void AlascaComponent::customEvent(QEvent * e)
|
---|
521 | {
|
---|
522 | //int type = e->type();
|
---|
523 |
|
---|
524 | AlascaFrame * frame = ((AlascaFrameEvent*)e)->frame;
|
---|
525 |
|
---|
526 | // we try to find some messages in the current packet + the pending bytes of the previous incoming data
|
---|
527 | splitPacket(frame->msg, frame->size, frame->time);
|
---|
528 |
|
---|
529 | // we delete the heap variable
|
---|
530 | delete frame;
|
---|
531 |
|
---|
532 | // we test if we have some messages to decode
|
---|
533 | while ( !msgList.empty() )
|
---|
534 | {
|
---|
535 | // get the first (the oldest) message and process it
|
---|
536 | unsigned long type = processMessage(msgList.front());
|
---|
537 |
|
---|
538 | if (type == ALASCA_SCANDATA_TYPE)
|
---|
539 | {
|
---|
540 | setState(ComponentBase::MONITOR_OK);
|
---|
541 | // write data on the disk
|
---|
542 | if (recording) writeData();
|
---|
543 | // push data in shared memory
|
---|
544 | shmem_->write(&alascaData, sizeof(alascaData));
|
---|
545 |
|
---|
546 | OutputInterface<LidarScan,AlascaComponent> * out = static_cast<OutputInterface<LidarScan,AlascaComponent> *> (output.value("scan"));
|
---|
547 | OutputInterface<ScanAlascaData,AlascaComponent> * outraw = static_cast<OutputInterface<ScanAlascaData,AlascaComponent> *> (output.value("raw"));
|
---|
548 | out->send(lidarScan);
|
---|
549 | outraw->send(alascaData);
|
---|
550 | }
|
---|
551 |
|
---|
552 | // removes the processed item of the list
|
---|
553 | msgList.pop_front();
|
---|
554 | }
|
---|
555 | }
|
---|
556 |
|
---|
557 | } // namespace pacpus
|
---|