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