source: pacpusframework/branches/2.0-beta1/src/TestComponents/Lidar/sensor/AlascaComponent.cpp@ 89

Last change on this file since 89 was 89, checked in by morasjul, 11 years ago

PACPUS 2.0 Beta deployed in new branch

Major changes:
-Add communication interface between components
-Add examples for communications interface (TestComponents)
-Move to Qt5 support

  • Property svn:executable set to *
File size: 19.7 KB
Line 
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
29using namespace std;
30
31namespace pacpus {
32
33DECLARE_STATIC_LOGGER("pacpus.base.AlascaComponent");
34
35// Construct the factory
36static ComponentFactory<AlascaComponent> sFactory("AlascaComponent");
37
38static const string kAlaskaMemoryName = "alasca";
39static const string kAlaskaDbtFileName = "alasca.dbt";
40static const string kAlaskaUtcFileName = "alasca_data.utc";
41
42static const long MagicWord = 0x55544300;
43
44static const int ALASCA_OBJECTDATA_TYPE = 1;
45static const int ALASCA_SCANDATA_TYPE = 15;
46
47void 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.
57AlascaComponent::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.
73AlascaComponent::~AlascaComponent()
74{
75 LOG_TRACE("destructor");
76 delete A_socket;
77}
78
79//////////////////////////////////////////////////////////////////////////
80/// Called by the ComponentManager to start the component
81void 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
115void 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
129ComponentBase::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
144long 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
175long 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
191bool 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
202void 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
230void 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
257void 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//////////////////////////////////////////////////////////////////////////
272void 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//////////////////////////////////////////////////////////////////////////
342short 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//////////////////////////////////////////////////////////////////////////
371float 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
385unsigned 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
487void 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.
520void 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
Note: See TracBrowser for help on using the repository browser.