source: pacpussensors/trunk/Sick/SickLDMRSSensor.cpp@ 54

Last change on this file since 54 was 52, checked in by cfougera, 10 years ago

Memory leak bug resolved (dynamic memory allocation is no longer used).
File size: 15.0 KB
Line 
1/*********************************************************************
2// created: 2014/02/02 - 12:08
3// filename: SickLDMRSSensor.cpp
4//
5// author: Cyril Fougeray
6// Copyright Heudiasyc UMR UTC/CNRS 6599
7//
8// version: $Id: $
9//
10// purpose: The acquisition class of the SickLDMRS sensor
11//
12*********************************************************************/
13
14#include "SickLDMRSSensor.h"
15
16#include "SickSocket.h"
17// #include "Pacpus/kernel/ComponentFactory.h"
18#include "Pacpus/kernel/DbiteException.h"
19#include "Pacpus/kernel/DbiteFileTypes.h"
20#include "Pacpus/kernel/Log.h"
21#include "Pacpus/PacpusTools/ShMem.h"
22
23#include <iostream>
24#include <QTcpSocket>
25#include <string>
26
27using namespace std;
28
29// #define SICKLDMRS_SH_MEM
30
31namespace pacpus {
32
33DECLARE_STATIC_LOGGER("pacpus.base.SickLDMRSSensor");
34
35
36static const string kSickMemoryName = "SickLDMRS";
37
38
39static const long MagicWord = 0xAFFEC0C2; // Sick LDMRS
40
41
42static const int SICKLDMRS_SCANDATA_TYPE = 0x2202;
43static const int SICKLDMRS_OBJECTDATA_TYPE = 0x2221;
44
45
46SickLDMRSSensor::SickLDMRSSensor(QObject *parent)
47 : ipaddr_("192.168.0.1"),
48 port_(12002),
49 name_("SickLDMRSDefault")
50{
51 LOG_TRACE("constructor ("<< this->name_ << "), default settings");
52
53 S_socket = new SickSocket(this);
54
55 connect(S_socket, SIGNAL(configuration()), this, SLOT(configure()) );
56
57 pendingBytes.time = 0;
58 pendingBytes.previousData = false;
59
60 this->kSickDbtFileName = name_.toStdString()+".dbt";
61 this->kSickUtcFileName = name_.toStdString()+"_data.utc";
62}
63
64SickLDMRSSensor::SickLDMRSSensor(QObject *parent, QString name, QString ip, int port, int recording)
65 : ipaddr_(ip),
66 port_(port),
67 name_(name),
68 recording_((bool)recording)
69{
70
71 LOG_TRACE("constructor ("<< this->name_ << "), configuration : " << this->ipaddr_<<":"<<this->port_);
72
73 S_socket = new SickSocket(this);
74
75 connect(S_socket, SIGNAL(configuration()), this, SLOT(configure()) );
76
77 pendingBytes.time = 0;
78 pendingBytes.previousData = false;
79
80 this->kSickDbtFileName = name_.toStdString()+".dbt";
81 this->kSickUtcFileName = name_.toStdString()+"_data.utc";
82}
83
84
85SickLDMRSSensor::~SickLDMRSSensor()
86{
87 LOG_TRACE("destructor ("<< this->name_ << ")");
88 delete S_socket;
89}
90
91
92void SickLDMRSSensor::startActivity()
93{
94
95 LOG_TRACE("Start Activity ("<< this->name_ <<")");
96
97 S_socket->connectToServer(ipaddr_, port_);
98
99 if (recording_) {
100 LOG_TRACE("(Recording) Starting recording...");
101
102 try {
103 dbtFile_.open(kSickDbtFileName, WriteMode, TELEM_SICK_LDMRS, sizeof(SickLDMRS_dbt));
104 } catch (DbiteException & e) {
105 cerr << "error opening dbt file: "<< kSickDbtFileName << ", " << e.what() << endl;
106 return;
107 }
108
109 // FIXME: use ofstream
110 // open the file with C function to be sure that it will exist
111 FILE * stream;
112 if (NULL == (stream = fopen(kSickUtcFileName.c_str(), "a+"))) {
113 LOG_FATAL("cannot open file '" << kSickUtcFileName.c_str() << "'");
114 ::exit(-1);
115 } else {
116 fclose(stream);
117 }
118
119 dataFile_.open(kSickUtcFileName.c_str(), ios_base::out|ios_base::binary|ios_base::app);
120 if (!dataFile_) {
121 LOG_FATAL("cannot open file '" << kSickUtcFileName.c_str() << "'");
122 ::exit(-1);
123 }
124 }
125 else
126 LOG_TRACE("(Recording) Not recording...");
127
128
129#ifdef SICKLDMRS_SH_MEM
130 shmem_ = new ShMem(kSickMemoryName.c_str(), sizeof(ScanSickData));
131#endif
132
133}
134
135
136void SickLDMRSSensor::stopActivity()
137{
138 LOG_TRACE("destructor (" << this->name_ << ")");
139
140 S_socket->sendToServer(QString((u_int32_t)0x0021));
141
142 S_socket->closeSocket();
143
144 if (recording_) {
145 LOG_TRACE("(Recording) Recording stopped");
146 dbtFile_.close();
147 dataFile_.close();
148 }
149
150#ifdef SICKLDMRS_SH_MEM
151 delete shmem_;
152#endif
153 // delete generator;
154
155}
156
157
158
159u_int32_t SickLDMRSSensor::findMagicWord(const char * message, const unsigned length)
160{
161 if (length < 4) {
162 return -1;
163 }
164
165 unsigned long i = 0;
166 while(*((u_int32_t*)(message+i)) != 0xC2C0FEAF){ // BigE
167 if (i == length) {
168 return -1;
169 }
170 ++i;
171 }
172 return i;
173
174}
175
176
177u_int32_t SickLDMRSSensor::getMessageSize(const char * message, const unsigned length, const long magicWordIndex)
178{
179
180 // we need at least 12 bytes
181 if (length < 12) {
182 return 0;
183 }
184 return ((*(message+magicWordIndex+11))&0x000000FF)
185 + ((*(message+magicWordIndex+10)<<8)&0x0000FF00)
186 + ((*(message+magicWordIndex+9)<<16)&0x00FF0000)
187 + ((*(message+magicWordIndex+8)<<24)&0xFF000000)
188 + 24 ;
189}
190
191
192bool SickLDMRSSensor::isMessageComplete(const unsigned length, const long size)
193{
194 if (size <= length) {
195 return true;
196 }
197 return false;
198}
199
200
201void SickLDMRSSensor::fillScanHeader( MessageLDMRS &msg )
202{
203 // address = base + Data header size (24-byte long) + offset
204 msg.hScan.scanNumber = *((u_int16_t*)(msg.body+24));
205 msg.hScan.scannerStatus = *((u_int16_t*)(msg.body+24+2));
206 msg.hScan.phaseOffset = *((u_int16_t*)(msg.body+24+4));
207 msg.hScan.startNtpTime = *((u_int64_t*)(msg.body+24+6));
208 msg.hScan.endNtpTime = *((u_int64_t*)(msg.body+24+14));
209 msg.hScan.ticksPerRot= *((u_int16_t*)(msg.body+24+22)); // needed to compute angle (°)
210 msg.hScan.startAngle = *((int16_t*)(msg.body+24+24));
211 msg.hScan.endAngle = *((int16_t*)(msg.body+24+26));
212 msg.hScan.numPoints = *((u_int16_t*)(msg.body+24+28));
213
214// msg.hScan.mountingYawAngle = *((int16_t*)(msg.body+24+30));
215// msg.hScan.mountingPitchAngle = *((int16_t*)(msg.body+24+32));
216// msg.hScan.mountingRollAngle = *((int16_t*)(msg.body+24+34));
217// msg.hScan.mountingX = *((int16_t*)(msg.body+24+36));
218// msg.hScan.mountingY = *((int16_t*)(msg.body+24+38));
219// msg.hScan.mountingZ = *((int16_t*)(msg.body+24+40));
220
221
222 LOG_TRACE("(hScan) Scan header data parsed, scan number " << msg.hScan.scanNumber);
223 LOG_TRACE("(hScan) Number of scanned points : " << msg.hScan.numPoints);
224}
225
226
227void SickLDMRSSensor::fillDataHeader(MessageLDMRS &msg)
228{
229
230 msg.hData.magicWord = ((*(msg.body+3))&0x000000FF) +
231 ((*(msg.body+2)<<8)&0x0000FF00) +
232 ((*(msg.body+1)<<16)&0x00FF0000)+
233 ((*(msg.body)<<24)&0xFF000000);
234 LOG_TRACE("(hData) Magic word read "<<msg.hData.magicWord);
235 // TODO check if OK
236
237 msg.hData.sizePreviousMessage = ((*(msg.body+7))&0x000000FF)+
238 ((*(msg.body+6)<<8)&0x0000FF00)+
239 ((*(msg.body+5)<<16)&0x00FF0000)+
240 ((*(msg.body+4)<<24)&0xFF000000);
241 LOG_TRACE("(hData) previous message size : "<<msg.hData.sizePreviousMessage);
242
243 msg.hData.sizeCurrentMessage =((*(msg.body+11))&0x000000FF)+
244 ((*(msg.body+10)<<8)&0x0000FF00)+
245 ((*(msg.body+9)<<16)&0x00FF0000)+
246 ((*(msg.body+8)<<24)&0xFF000000);
247
248 LOG_TRACE("(hData) current message size : "<<msg.hData.sizeCurrentMessage);
249
250
251 msg.hData.deviceId = *(msg.body+13);
252
253 msg.hData.dataType = ((*(msg.body+15))&0x000000FF)+((*(msg.body+14)<<8)&0x0000FF00);
254 LOG_TRACE("(hData) Data type : " << msg.hData.dataType);
255
256
257 msg.hData.ntpTime = ((*(msg.body+15))&0x000000FF)+
258 ((*(msg.body+14)<<8)&0x0000FF00)+
259 ((*(msg.body+13)<<16)&0x00FF0000)+
260 ((*(msg.body+12)<<24)&0xFF000000)+
261 ((*(msg.body+11))&0x000000FF)+
262 ((*(msg.body+10)<<8)&0x0000FF00)+
263 ((*(msg.body+9)<<16)&0x00FF0000)+
264 ((*(msg.body+8)<<24)&0xFF000000);
265}
266
267
268void SickLDMRSSensor::storePendingBytes(road_time_t time)
269{
270 if (!pendingBytes.previousData)
271 {
272 pendingBytes.time = time;
273 pendingBytes.previousData = true;
274 }
275}
276
277
278void SickLDMRSSensor:: splitPacket(const char * packet, const int length, road_time_t time)
279{
280 long index = 0;
281 long msgSize = 0;
282 bool msgComplete = false;
283
284 // we are working on the previous not decoded data + the actual incoming packet
285 pendingBytes.data.append(packet,length);
286 LOG_TRACE("(Packet reconstitution) Pending bytes : " << pendingBytes.data.size() );
287
288
289 while (pendingBytes.data.size() > 0)
290 {
291 // we are looking for the MagicWord
292 index = findMagicWord(pendingBytes.data.c_str() , pendingBytes.data.size() );
293 LOG_TRACE("(Packet reconstitution) MagicWord index : " << index );
294 if (index == -1)
295 {
296 storePendingBytes(time);
297 // exit the while loop
298 break;
299 }
300
301
302 // we are looking for the size of the message (scan data + scan points)
303 msgSize = getMessageSize(pendingBytes.data.c_str(), pendingBytes.data.size(), index);
304 if (msgSize == 0)
305 {
306 storePendingBytes(time);
307 // exit the while loop
308 break;
309 }
310
311 LOG_TRACE("(Packet reconstitution) Message size (data header(24) + size of the message) : " << msgSize );
312
313 // we are verifying if the message is complete
314 msgComplete = isMessageComplete( pendingBytes.data.size() , msgSize );
315 if (msgComplete == false)
316 {
317 storePendingBytes(time);
318 // exit the while loop
319 break;
320 }
321
322 LOG_TRACE("(Packet reconstitution) Message complete ! ");
323 // we have a complete message available that we can add to the list
324 MessageLDMRS msg;
325
326 // we copy the bytes in the body message
327// char* messageData = (char*)malloc(msgSize);
328// if(messageData == NULL){
329// LOG_FATAL("(Packet reconstitution) Malloc FAILED. Packet lost.");
330// return;
331// }
332 memcpy(msg.body, pendingBytes.data.c_str() + index, msgSize);
333
334// msg->body = messageData;
335
336 // we set the timestamp of the message
337 if (pendingBytes.previousData)
338 {
339 // the timestamp is the one of the previous packet
340 msg.time = pendingBytes.time;
341 pendingBytes.previousData = false;
342 }
343 else
344 {
345 // the timestamp is the one of the actual received packet
346 msg.time = time;
347 }
348
349 // we add the message to the list
350 msgList.push_back(msg);
351 // and we suppress the processed bytes of the pending data
352 pendingBytes.data.erase(0, msgSize);
353 }
354}
355
356
357unsigned long SickLDMRSSensor::processMessage(MessageLDMRS &msg)
358{
359 fillDataHeader(msg);
360 if (SICKLDMRS_SCANDATA_TYPE == msg.hData.dataType) {
361 LOG_TRACE("(Process Message) Scan Data Type!");
362 fillScanHeader(msg);
363
364 int index = 24 + 44; // data header + scan header
365
366 if(sizeof(ScanPoint) * msg.hScan.numPoints > BODY_MAX_SIZE){
367 LOG_FATAL("Size of the message is too long !");
368 return 0;
369 }
370
371 ScanPoint scanPoints[msg.hScan.numPoints];
372
373 // replace memory with structured data
374 for (int i = 0; i < msg.hScan.numPoints; ++i) {
375 scanPoints[i].layerEcho = *((uchar*)(msg.body + index));
376 scanPoints[i].flags = *((uchar*)(msg.body + index + 1));
377 scanPoints[i].angle = *((u_int16_t*)(msg.body + index + 2));
378 scanPoints[i].distance = *((u_int16_t*)(msg.body + index + 4));
379 scanPoints[i].echoPulseWidth = *((u_int16_t*)(msg.body + index + 6));
380 }
381
382 memcpy(msg.body, scanPoints, sizeof(ScanPoint) * msg.hScan.numPoints);
383 }
384 else if (msg.hData.dataType == SICKLDMRS_OBJECTDATA_TYPE){
385 LOG_TRACE("(Process Message) Object Data Type!");
386
387 // TODO
388 }
389 else {// irrelevant data type
390 // TODO
391 }
392
393
394 return msg.hData.dataType;
395}
396
397
398void SickLDMRSSensor::writeData(MessageLDMRS &msg)
399{
400 // record the data in a dbt file. The dbt data is only the structure DataHeader
401 // scan data are recorded in the sickldmrs_data.utc file
402
403 SickLDMRS_dbt entry;
404 entry.timeStartFromSensor = msg.hData.ntpTime;
405 entry.hScan = msg.hScan;
406 entry.dataPos = dataFile_.tellp(); // absolute position of pointer in UTC file
407// entry.time = msg.time;
408// entry.timerange = msg.timerange;
409
410 LOG_TRACE("Writing into DBT + UTC files..");
411
412 // write DBT
413 try {
414 dbtFile_.writeRecord(msg.time, msg.timerange, (char *) &entry, sizeof(SickLDMRS_dbt));
415 } catch (DbiteException & e) {
416 cerr << "error writing data: " << e.what() << endl;
417 return;
418 }
419
420 // record the scan data in a binary file sickldmrs_data.utc with "UTC\0" to separate the data
421 for (unsigned int i = 0 ; i < msg.hScan.numPoints ; ++i) {
422 dataFile_.write((msg.body + i*sizeof(ScanPoint)), sizeof(ScanPoint));
423 }
424 // add a magic word to delimit the block of data
425 int32_t utcMagicWord = UTC_MAGIC_WORD;
426 dataFile_.write(reinterpret_cast<char*>(&(utcMagicWord)), sizeof(int32_t));
427
428 LOG_TRACE("Writing done !");
429
430}
431
432
433
434void SickLDMRSSensor::configure(){
435 // Start measuring
436 // S_socket->sendToServer(QString((u_int32_t)0x0020));
437
438 // LOG_TRACE(this->name_ +" configured.");
439}
440
441
442void SickLDMRSSensor::customEvent(QEvent * e)
443{
444 SickFrame * frame = ((SickFrameEvent*)e)->frame;
445
446 // we try to find some messages in the current packet + the pending bytes of the previous incoming data
447 splitPacket(frame->msg, frame->size, frame->time);
448
449 // we delete the heap variable
450 delete frame;
451
452 // we test if we have some messages to decode
453 while ( !msgList.empty() )
454 {
455 LOG_TRACE("Message waiting");
456 // get the first (the eldest) message and process it
457 MessageLDMRS msgToProcess = msgList.front();
458 unsigned long type = processMessage(msgToProcess);
459 LOG_TRACE("Message processed !");
460
461 if (type == SICKLDMRS_SCANDATA_TYPE)
462 {
463// setState(ComponentBase::MONITOR_OK);
464 // write data on the disk
465 if (recording_)
466 writeData(msgToProcess);
467
468#ifdef SICKLDMRS_SH_MEM
469 /// NOT TESTED !
470 /// push data in shared memory
471 // First the scan info
472 SickLDMRS_shMem toWrite;
473 toWrite.time = msgToProcess.time;
474 toWrite.timerange = msgToProcess.timerange;
475 toWrite.scanInfo = msgToProcess.hScan;
476 shmem_->write(toWrite, sizeof(SickLDMRS_shMem));
477
478 // Then, the points
479 for (unsigned int i = 0 ; i < msgToProcess.hScan.numPoints ; ++i) {
480 shmem_->write((msg.body + i*sizeof(ScanPoint)), sizeof(ScanPoint));
481 }
482#endif
483
484 }
485 else if (type == SICKLDMRS_OBJECTDATA_TYPE)
486 {
487 // Handled via CAN bus ?
488 }
489
490 // removes the processed item of the list
491 // free(msgList.front().body);
492 msgList.pop_front();
493 }
494}
495
496} // namespace pacpus
Note: See TracBrowser for help on using the repository browser.