source: pacpussensors/trunk/Alasca/AlascaComponent.cpp@ 77

Last change on this file since 77 was 66, checked in by DHERBOMEZ Gérald, 10 years ago

Modifications:

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