source: pacpussensors/trunk/Sick/SickLMSSensor.cpp@ 39

Last change on this file since 39 was 37, checked in by cfougera, 11 years ago

First commit of Sick lidars interfaces.
File size: 26.7 KB
Line 
1/*********************************************************************
2// created: 2014/02/11 - 12:08
3// filename: SickLMSSensor.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 SickLMS sensor
11//
12*********************************************************************/
13
14#include "SickLMSSensor.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 <boost/algorithm/string/split.hpp>
24#include <boost/algorithm/string/classification.hpp>
25
26#include <iostream>
27#include <QTcpSocket>
28#include <string>
29#include <vector>
30
31using namespace std;
32
33// #define SICKLMS_SH_MEM
34
35namespace pacpus {
36
37static const int STX_CHAR = 0x02;
38static const int ETX_CHAR = 0x03;
39
40DECLARE_STATIC_LOGGER("pacpus.base.SickLMSSensor");
41
42// Commands which can be taken into account
43static const std::string SICKLMS_SCANDATA_MESSAGE = "LMDscandata";
44static const std::string SICKLMS_SCANCONFIG_MESSAGE = "LMPscancfg";
45static const int MINIMUM_PARAMETERS_MSG = 6; // ScanConfig message is the smallest relevant message
46
47//////////////////////////////////////////////////////////////////////////
48/// Constructor.
49SickLMSSensor::SickLMSSensor(QObject *parent)
50 : ipaddr_("192.168.0.50"),
51 port_(1211),
52 name_("SickLMSDefault"),
53 recording_(false)
54{
55 LOG_TRACE("constructor ("<<this->name_ << "), default settings");
56
57 S_socket = new SickSocket(this);
58
59 connect(S_socket, SIGNAL(configuration()), this, SLOT(configure()) );
60
61 pendingBytes.time = 0;
62 pendingBytes.previousData = false;
63
64 this->kSickDbtFileName = name_.toStdString()+".dbt";
65 this->kSickUtcFileName = name_.toStdString()+"_data.utc";
66}
67
68SickLMSSensor::SickLMSSensor(QObject *parent, QString name, QString ip, int port, int recording)
69 : ipaddr_(ip),
70 port_(port),
71 name_(name),
72 recording_((bool)recording)
73{
74
75 LOG_TRACE("constructor ("<< this->name_ << "), configuration : " << this->ipaddr_<<":"<<this->port_);
76
77 S_socket = new SickSocket(this);
78
79 connect(S_socket, SIGNAL(configuration()), this, SLOT(configure()) );
80
81 pendingBytes.time = 0;
82 pendingBytes.previousData = false;
83
84 this->kSickDbtFileName = name_.toStdString()+".dbt";
85 this->kSickUtcFileName = name_.toStdString()+"_data.utc";
86}
87
88//////////////////////////////////////////////////////////////////////////
89/// Destructor.
90SickLMSSensor::~SickLMSSensor()
91{
92 LOG_TRACE("destructor ("<< this->name_ << ")");
93 delete S_socket;
94}
95
96//////////////////////////////////////////////////////////////////////////
97/// Called by the ComponentManager to start the component
98void SickLMSSensor::startActivity()
99{
100
101 LOG_TRACE("Start Activity ("<< this->name_ <<")");
102
103 S_socket->connectToServer(ipaddr_, port_);
104
105 // config can be computed for each measure
106 // askScanCfg();
107
108
109 char buf[100];
110
111// ///LOGIN
112// sprintf(buf, "%c%s%c", 0x02, "sMN SetAccessMode 03 F4724744", 0x03);
113// S_socket->sendToServer(QString(buf));
114
115
116// sprintf(buf, "%c%s%c", 0x02, "sWN LMDscandatacfg 03 00 1 1 0 00 00 0 0 0 0 1 1", 0x03);
117// S_socket->sendToServer(QString(buf));
118
119//// sprintf(buf, "%c%s%c", 0x02, "sWN FREchoFilter 1", 0x03);
120//// S_socket->sendToServer(QString(buf));
121
122/////// SAVE PERMANENT
123// sprintf(buf, "%c%s%c", 0x02, "sMN mEEwriteall", 0x03);
124// S_socket->sendToServer(QString(buf));
125
126//// /// LOGOUT
127// sprintf(buf, "%c%s%c", 0x02, "sMN Run", 0x03);
128// S_socket->sendToServer(QString(buf));
129
130
131 // Start measurement
132 sprintf(buf, "%c%s%c", 0x02, "sEN LMDscandata 1", 0x03);
133 S_socket->sendToServer(QString(buf));
134
135 /// TODO get response from sensor and analyse it to know if measuring has started
136 /// See p23 telegram
137 if(0)
138 LOG_INFO("(Measuring) Data sent permanently");
139
140
141 if (recording_) {
142 LOG_INFO("(Recording) Recording is on.");
143
144 try {
145 dbtFile_.open(kSickDbtFileName, WriteMode, TELEM_SICK, sizeof(SickLMS_dbt));
146 } catch (DbiteException & e) {
147 cerr << "error opening dbt file: "<< kSickDbtFileName << ", " << e.what() << endl;
148 return;
149 }
150
151 // FIXME: use ofstream
152 // open the file with C function to be sure that it will exist
153 FILE * stream;
154 if (NULL == (stream = fopen(kSickUtcFileName.c_str(), "a+"))) {
155 LOG_FATAL("cannot open file '" << kSickUtcFileName.c_str() << "'");
156 ::exit(-1);
157 } else {
158 fclose(stream);
159 }
160
161 dataFile_.open(kSickUtcFileName.c_str(), ios_base::out|ios_base::binary|ios_base::app);
162 if (!dataFile_) {
163 LOG_FATAL("cannot open file '" << kSickUtcFileName.c_str() << "'");
164 ::exit(-1);
165 }
166 }
167 else
168 LOG_INFO("(Recording) Not recording.");
169
170#ifdef SICKLMS_SH_MEM
171 shmem_ = new ShMem(kSickMemoryName.c_str(), sizeof(ScanSickData));
172#endif
173
174}
175
176//////////////////////////////////////////////////////////////////////////
177/// Called by the ComponentManager to stop the component
178void SickLMSSensor::stopActivity()
179{
180 LOG_TRACE("destructor (" << this->name_ << ")");
181
182 // Stop measurement. Not necessary : when socket is closed, data is no longer sent from sensor
183 char buf[64];
184 sprintf(buf, "%c%s%c", 0x02, "sEN LMDscandata 0", 0x03);
185 S_socket->sendToServer(QString(buf));
186
187 S_socket->closeSocket();
188
189 if (recording_) {
190 LOG_TRACE("(Recording) Recording stopped");
191 dbtFile_.close();
192 dataFile_.close();
193 }
194
195#ifdef SICKLMS_SH_MEM
196 delete shmem_;
197#endif
198 // delete generator;
199
200}
201
202//////////////////////////////////////////////////////////////////////////
203///
204void SickLMSSensor::configure(){
205 // Start measuring
206 // S_socket->sendToServer(QString((u_int32_t)0x0020));
207
208 // LOG_TRACE(this->name_ +" configured.");
209}
210
211
212
213
214/// Data must be parsed in slot customEvent
215void SickLMSSensor::askScanCfg(){
216 char buf[100];
217 sprintf(buf, "%c%s%c", 0x02, "sRN LMPscancfg", 0x03);
218
219 S_socket->sendToServer(QString(buf));
220}
221
222
223
224//////////////////////////////////////////////////////////////////////////
225///
226int SickLMSSensor::isMessageComplete(const char* packets, unsigned int size)
227{
228 for(int i = 0; i < size; ++i){
229 if(packets[i] == ETX_CHAR)
230 return i;
231 }
232 return -1;
233}
234
235
236//////////////////////////////////////////////////////////////////////////
237/// look for the position of the Start of Text character
238/// Return it if found else return -1
239int SickLMSSensor::findSTX(const char* packets, const unsigned int size ){
240 int i = 0;
241 while(i < size){
242 if(packets[i] == STX_CHAR)
243 return i;
244 i++;
245 }
246 return -1;
247}
248
249
250//////////////////////////////////////////////////////////////////////////
251/// this function is called when no complete message has been found
252/// we set the flag previousData to true for the next processing stage
253/// and we store the timestamp of the bytes acquisition
254void SickLMSSensor::storePendingBytes(road_time_t time)
255{
256 if (!pendingBytes.previousData)
257 {
258 pendingBytes.time = time;
259 pendingBytes.previousData = true;
260 }
261}
262
263//////////////////////////////////////////////////////////////////////////
264/// Analyse the ethernet packet received from the Sick LMS and try to find a
265/// complete message (scan data message)
266/// If a message has been found it is added at the end of the message list
267/// else the pending bytes are stored to be analyzed by further icoming data
268//////////////////////////////////////////////////////////////////////////
269void SickLMSSensor::reconstituteMessage(const char * packet, const int length, road_time_t time)
270{
271 long indexSTX = 0;
272 long indexETX = 0;
273 long msgSize = 0;
274
275 // we are working on the previous not decoded data + the actual incoming packet
276 pendingBytes.data.append(packet,length);
277 LOG_TRACE("(Packet reconstitution) Pending bytes : " << pendingBytes.data.size() );
278
279
280 while (pendingBytes.data.size() > 0)
281 {
282 // we are looking for the start of frame <STX> (= 0x02)
283 indexSTX = findSTX(pendingBytes.data.c_str() , pendingBytes.data.size() );
284 LOG_TRACE("(Packet reconstitution) Start of text index : " << indexSTX );
285 if (indexSTX == -1)
286 {
287 storePendingBytes(time);
288 // exit the while loop
289 break;
290 }
291
292 // we are verifying if the message is complete
293 indexETX = isMessageComplete(pendingBytes.data.c_str(), pendingBytes.data.size());
294 LOG_TRACE("(Packet reconstitution) End of text index : " << indexETX );
295 if (indexETX == -1)
296 {
297 storePendingBytes(time);
298 // exit the while loop
299 break;
300 }
301
302 // we have a complete message available that we can add to the list
303 MessageLMS msg;
304
305 // we copy the bytes in the body message
306 msgSize = indexETX - indexSTX + 1;
307 char* messageData = (char*)malloc(msgSize+1);
308 memcpy(messageData, pendingBytes.data.c_str() + indexSTX, msgSize);
309
310 msg.body = messageData;
311 msg.msgSize = msgSize;
312
313 // we set the timestamp of the message
314 if (pendingBytes.previousData)
315 {
316 // the timestamp is the one of the previous packet
317 msg.time = pendingBytes.time;
318 pendingBytes.previousData = false;
319 }
320 else
321 {
322 // the timestamp is the one of the actual received packet
323 msg.time = time;
324 }
325
326 // we add the message to the list
327 msgList.push_back(msg);
328 // and we suppress the processed bytes of the pending data
329 pendingBytes.data.erase(0, msgSize);
330 }
331}
332
333
334/**
335 * \fn
336 * \brief
337 * \param
338 * \param
339 * \return
340 */
341int SickLMSSensor::splitMessage(MessageLMS* message){
342
343 message->splitMessage = new std::vector<std::string>();
344
345 for(int i; i < message->msgSize; ++i){
346 std::string* str = new std::string();
347 while(message->body[i] != ' ' && i < message->msgSize){
348 str->push_back(message->body[i]);
349 ++i;
350 }
351 message->splitMessage->push_back(*str);
352 delete str;
353 }
354 LOG_TRACE("(splitMessage) Number of parameters into the message : "<< (int)message->splitMessage->size());
355
356 return (int) message->splitMessage->size();
357}
358
359//////////////////////////////////////////////////////////////////////////
360/// Convert hexadecimal value stored as ASCII string to long
361/// Warning : alpha characters must be in capital letters
362long SickLMSSensor::xstol(std::string str){
363 long ret = 0;
364 for (std::string::iterator it=str.begin(); it!=str.end(); ++it){
365 if(*it >= 'A' && *it <= 'F'){
366 ret *= 16;
367 ret += (*it - 'A' + 10);
368 }
369 else if (*it >= '0' && *it <= '9'){
370 ret *= 16;
371 ret += (*it - '0');
372 }
373 else
374 LOG_WARN("(conversion) String is not a hex value");
375 }
376 return ret;
377}
378
379
380int SickLMSSensor::processScanData(MessageLMS* msg)
381{
382 // just non-empty arrays will be saved in DBT
383 // so first, we initialize these values to 0 and then fill them if message contains corresponding data
384 msg->data.dist_len1 = 0;
385 msg->data.dist_len2 = 0;
386 msg->data.dist_len3 = 0;
387 msg->data.dist_len4 = 0;
388 msg->data.dist_len5 = 0;
389 msg->data.rssi_len1 = 0;
390 msg->data.rssi_len2 = 0;
391
392
393// for(std::vector<std::string>::iterator it = msg->splitMessage->begin(); it != msg->splitMessage->end(); it++){
394// printf("%s ", (*it).c_str());
395// }
396// printf("\n");
397
398 // 0 //Type of command
399 // 1 //Command
400 // 2 //VersionNumber
401 // 3 //DeviceNumber
402 // 4 //Serial number
403 // 5-6 //DeviceStatus
404 // 00 00 OK
405 // 00 01 Error
406 // 00 02 Pollution Warning
407 // 00 04 Pollution Error
408
409 // 7 //MessageCounter
410
411 LOG_TRACE("(Parsing) Message number 0x"<< msg->splitMessage->at(7).c_str());
412
413 // 8 //ScanCounter
414 // 9 //PowerUpDuration
415 // 10 //TransmissionDuration
416 // 11-12 //InputStatus
417 // 13-14 //OutputStatus
418 // 15 //ReservedByteA
419 // 16 //ScanningFrequency
420 msg->data.scanFrequency = xstol(msg->splitMessage->at(16));
421 LOG_TRACE("(Parsing) Scan frequency "<< msg->data.scanFrequency <<" [1/100 Hz]");
422
423 // 17 //MeasurementFrequency
424
425 // 18 //NumberEncoders
426 int NumberEncoders = xstol(msg->splitMessage->at(18));
427 LOG_TRACE("(Parsing) Number Encoders "<< NumberEncoders);
428
429 for (int i = 0; i < NumberEncoders; i++) {
430 // //EncoderPosition
431 // //EncoderSpeed
432 }
433
434
435 // 18+NumberEncoders*2+1 //NumberChannels16Bit
436 int NumberChannels16Bit = xstol(msg->splitMessage->at(18+NumberEncoders*2+1));
437 LOG_TRACE("(Parsing) Number channels 16Bit : "<<NumberChannels16Bit);
438
439 int totalData16 = 0;
440
441 for (int i = 0; i < NumberChannels16Bit; i++) {
442 int type = -1; // 0 DIST1 1 DIST2 2 (LMS1xx & LMS5xx)
443 // RSSI1 3 RSSI2 (LMS1xx)
444 // 4 DIST3 5 DIST4 6 DIST5 (LMS5xx)
445
446
447 int NumberData;
448
449 std::string content = msg->splitMessage->at(19+NumberEncoders*2+i*6+totalData16+1);
450 // 19+NumberEncoders*2+i*6+totalData16+1 //MeasuredDataContent
451
452 LOG_TRACE("(Parsing 16bit channel #"<<i<<") Measured Data Content : " << content);
453 if (content == "DIST1") {
454 type = 0;
455 } else if (content == "DIST2") {
456 type = 1;
457 } else if (content == "RSSI1") {
458 type = 2;
459 } else if (content == "RSSI2") {
460 type = 3;
461 } else if (content == "DIST3") {
462 type = 4;
463 } else if (content == "DIST4") {
464 type = 5;
465 } else if (content == "DIST5") {
466 type = 6;
467 }
468
469// 19+NumberEncoders*2+i*6+totalData16+2 //ScalingFactor
470 int scalingFactor = 1;
471 if(msg->splitMessage->at(19+NumberEncoders*2+i*6+totalData16+2) == "40000000")
472 scalingFactor = 2;
473 LOG_TRACE("(Parsing 16bit channel #"<<i<<") Scaling factor x"<< scalingFactor);
474
475// 19+NumberEncoders*2+i*6+totalData16+3 //ScalingOffset
476// 19+NumberEncoders*2+i*6+totalData16+4 //Starting angle
477 msg->data.startAngle = xstol(msg->splitMessage->at(19+NumberEncoders*2+i*6+totalData16+4));
478 LOG_TRACE("(Parsing 16bit channel #"<<i<<") Start angle "<< msg->data.startAngle << " [1/10000 degree]");
479
480// 19+NumberEncoders*2+i*6+totalData16+5 //Angular step width
481 msg->data.angleResolution = xstol(msg->splitMessage->at(19+NumberEncoders*2+i*6+totalData16+5));
482 LOG_TRACE("(Parsing 16bit channel #"<<i<<") Angular step width "<< msg->data.angleResolution<<" [1/10000 degree]");
483
484// 19+NumberEncoders*2+i*6+totalData16+6 //NumberData
485 NumberData = xstol(msg->splitMessage->at(19+NumberEncoders*2+i*6+totalData16+6));
486
487 LOG_TRACE("(Parsing 16bit channel #"<<i<<") Number Data for "<<content<<" : "<<NumberData);
488
489 LOG_TRACE("(Parsing 16bit channel #"<<i<<") First data "<<content<<" (index "<<(19+NumberEncoders*2+i*6+totalData16+7)<<") : "<<xstol(msg->splitMessage->at(19+NumberEncoders*2+i*6+totalData16+7)));
490 LOG_TRACE("(Parsing 16bit channel #"<<i<<") Last data "<<content<<" (index "<<(19+NumberEncoders*2+i*6+totalData16+7+NumberData-1)<<") : "<<xstol(msg->splitMessage->at(19+NumberEncoders*2+i*6+totalData16+7+NumberData-1)));
491
492 uint16_t* distPoints = (uint16_t*) malloc(NumberData * sizeof(uint16_t));
493 for (int j = 0; j < NumberData; j++) {
494 distPoints[j] = scalingFactor * xstol(msg->splitMessage->at(19+NumberEncoders*2+i*6+totalData16+7+j));
495 }
496 if (type == 0) {
497 msg->data.dist1 = distPoints;
498 msg->data.dist_len1 = NumberData;
499 } else if (type == 1) {
500 msg->data.dist2 = distPoints;
501 msg->data.dist_len2 = NumberData;
502 } else if (type == 2) {
503 msg->data.rssi1 = distPoints;
504 msg->data.rssi_len1 = NumberData;
505 } else if (type == 3) {
506 msg->data.rssi2 = distPoints;
507 msg->data.rssi_len2 = NumberData;
508 } else if (type == 4) {
509 msg->data.dist3 = distPoints;
510 msg->data.dist_len3 = NumberData;
511 } else if (type == 5) {
512 msg->data.dist4 = distPoints;
513 msg->data.dist_len4 = NumberData;
514 } else if (type == 6) {
515 msg->data.dist5 = distPoints;
516 msg->data.dist_len5 = NumberData;
517 }
518 totalData16 += NumberData;
519
520 }
521
522
523// 19+NumberEncoders*2+NumberChannels16Bit*6+totalData16+1 //NumberChannels8Bit
524 int NumberChannels8Bit = xstol(msg->splitMessage->at(19+NumberEncoders*2+NumberChannels16Bit*6+totalData16+1));
525 LOG_TRACE("(Processing) Number channels 8Bit : "<<NumberChannels8Bit);
526
527 LOG_INFO("(Parsing) 8bit channel not implemented yet !");
528
529/*
530// int totalData8 = 0;
531// for (int i = 0; i < NumberChannels8Bit; i++) {
532
533// int type = -1;
534
535// std::string content = msg->splitMessage->at(21+NumberEncoders*2+NumberChannels16Bit*6+totalData16+totalData8+i*6);
536// LOG_TRACE("(Parsing 8bit channel) Measured Data Content : " << content);
537// if (content == "DIST1") {
538// type = 0;
539// } else if (content=="DIST2") {
540// type = 1;
541// } else if (content=="RSSI1") {
542// type = 2;
543// } else if (content == "RSSI2") {
544// type = 3;
545// }
546
547// 21+NumberEncoders*2+NumberChannels16Bit*6+totalData16+i*6+1 //ScalingFactor
548// 21+NumberEncoders*2+NumberChannels16Bit*6+totalData16+i*6+2 //ScalingOffset
549// 21+NumberEncoders*2+NumberChannels16Bit*6+totalData16+i*6+3 //Starting angle
550// 21+NumberEncoders*2+NumberChannels16Bit*6+totalData16+i*6+4 //Angular step width
551// 21+NumberEncoders*2+NumberChannels16Bit*6+totalData16+i*6+5 //NumberData
552
553// int NumberData = xstol(msg->splitMessage->at(21+NumberEncoders*2+NumberChannels16Bit*6+totalData16+totalData8+i*6+5));
554// LOG_TRACE("(Parsing 16bit channel) Number Data for "<<content<<" : "<<NumberData);
555
556// sscanf(tok, "%X", &NumberData);
557
558// LOG_TRACE("(Processing) Number data : "<< NumberData);
559
560// if (type == 0) {
561// msg->data.dist_len1 = NumberData;
562// } else if (type == 1) {
563// msg->data.dist_len2 = NumberData;
564// } else if (type == 2) {
565// msg->data.rssi_len1 = NumberData;
566// } else if (type == 3) {
567// msg->data.rssi_len2 = NumberData;
568// }
569// for (int i = 0; i < NumberData; i++) {
570// int dat;
571// tok = strtok(NULL, " "); //data
572// sscanf(tok, "%X", &dat);
573
574// if (type == 0) {
575// msg->data.dist1[i] = dat;
576// } else if (type == 1) {
577// msg->data.dist2[i] = dat;
578// } else if (type == 2) {
579// msg->data.rssi1[i] = dat;
580// } else if (type == 3) {
581// msg->data.rssi2[i] = dat;
582// }
583// }
584 } // 8bit channel
585*/
586
587}
588
589//////////////////////////////////////////////////////////////////////////
590/// write the data on the disk:
591void SickLMSSensor::writeData(MessageLMS &msg)
592{
593 SickLMS_dbt entry;
594 entry.angleResolution = msg.data.angleResolution;
595 entry.scanNumber = xstol(msg.splitMessage->at(7));
596 entry.scannerStatus = xstol(msg.splitMessage->at(6));
597 entry.time = msg.time;
598 entry.timerange = msg.timerange;
599 entry.scanFrequency = msg.data.scanFrequency;
600 entry.angleResolution = msg.data.angleResolution;
601 entry.startAngle = msg.data.startAngle;
602
603 if(msg.data.dist_len1){
604 entry.dist_len1 = msg.data.dist_len1;
605 entry.dataPos_dist1 = dataFile_.tellp();
606
607 for (unsigned int i = 0 ; i < msg.data.dist_len1; ++i) {
608 dataFile_.write(reinterpret_cast<char*>(&(msg.data.dist1[i])), sizeof(u_int16_t));
609 }
610
611 free(msg.data.dist1);
612 }
613 if(msg.data.dist_len2){
614 entry.dist_len2 = msg.data.dist_len2;
615 entry.dataPos_dist2 = dataFile_.tellp();
616
617 for (unsigned int i = 0 ; i < msg.data.dist_len2; ++i) {
618 dataFile_.write(reinterpret_cast<char*>(&(msg.data.dist2[i])), sizeof(u_int16_t));
619 }
620
621 free(msg.data.dist2);
622 }
623 if(msg.data.dist_len3){
624 entry.dist_len3 = msg.data.dist_len3;
625 entry.dataPos_dist3 = dataFile_.tellp();
626
627 for (unsigned int i = 0 ; i < msg.data.dist_len3; ++i) {
628 dataFile_.write(reinterpret_cast<char*>(&(msg.data.dist3[i])), sizeof(u_int16_t));
629 }
630
631 free(msg.data.dist3);
632 }
633 if(msg.data.dist_len4){
634 entry.dist_len4 = msg.data.dist_len4;
635 entry.dataPos_dist4 = dataFile_.tellp();
636
637 for (unsigned int i = 0 ; i < msg.data.dist_len4; ++i) {
638 dataFile_.write(reinterpret_cast<char*>(&(msg.data.dist4[i])), sizeof(u_int16_t));
639 }
640
641 free(msg.data.dist4);
642 }
643 if(msg.data.dist_len5){
644 entry.dist_len5 = msg.data.dist_len5;
645 entry.dataPos_dist5 = dataFile_.tellp();
646
647 for (unsigned int i = 0 ; i < msg.data.dist_len5; ++i) {
648 dataFile_.write(reinterpret_cast<char*>(&(msg.data.dist5[i])), sizeof(u_int16_t));
649 }
650
651 free(msg.data.dist5);
652 }
653 if(msg.data.rssi_len1){
654 entry.rssi_len1 = msg.data.rssi_len1;
655 entry.dataPos_rssi1 = dataFile_.tellp();
656
657 for (unsigned int i = 0 ; i < msg.data.rssi_len1; ++i) {
658 dataFile_.write(reinterpret_cast<char*>(&(msg.data.rssi1[i])), sizeof(u_int16_t));
659 }
660
661 free(msg.data.rssi1);
662 }
663 if(msg.data.rssi_len2){
664 entry.rssi_len2 = msg.data.rssi_len2;
665 entry.dataPos_rssi2 = dataFile_.tellp();
666
667 for (unsigned int i = 0 ; i < msg.data.rssi_len2; ++i) {
668 dataFile_.write(reinterpret_cast<char*>(&(msg.data.rssi2[i])), sizeof(u_int16_t));
669 }
670
671 free(msg.data.rssi2);
672 }
673 // add a magic word to delimit the block of data
674 int32_t utcMagicWord = UTC_MAGIC_WORD;
675 dataFile_.write(reinterpret_cast<char*>(&(utcMagicWord)), sizeof(int32_t));
676
677 // write DBT
678 try {
679 dbtFile_.writeRecord(msg.time, msg.timerange, (char *) &entry, sizeof(SickLMS_dbt));
680 } catch (DbiteException & e) {
681 cerr << "error writing data: " << e.what() << endl;
682 return;
683 }
684
685}
686
687
688
689//////////////////////////////////////////////////////////////////////////
690/// Event call by the SickSocket class when new data has been arrived
691/// on the network.
692void SickLMSSensor::customEvent(QEvent * e)
693{
694 char answerMsg1[10];
695 sprintf(answerMsg1, "%c%s", 0x02, "sRA");
696 char answerMsg2[10];
697 sprintf(answerMsg2, "%c%s", 0x02, "sSN");
698
699 SickFrame * frame = ((SickFrameEvent*)e)->frame;
700
701 // we try to find some messages in the current packet + the pending bytes of the previous incoming data
702 reconstituteMessage(frame->msg, frame->size, frame->time);
703
704 // we delete the heap variable
705 delete frame;
706
707 // we test if we have some messages to decode
708 while ( !msgList.empty() )
709 {
710 // get the first (the eldest) message and process it
711 MessageLMS* msgToProcess = &(msgList.front());
712
713 // verify if the message is worth to be decoded
714 if(!strncmp(answerMsg1, msgToProcess->body, 4) || !strncmp(answerMsg2, msgToProcess->body, 4)){
715
716 if(splitMessage(msgToProcess) >= MINIMUM_PARAMETERS_MSG){
717
718 std::string type = msgToProcess->splitMessage->at(1);
719
720 LOG_TRACE("(Message type) "<< type);
721
722 if (type == SICKLMS_SCANDATA_MESSAGE)
723 {
724 LOG_TRACE("Scan data message !");
725
726 processScanData(msgToProcess);
727
728 // write data on the disk (dbt + utc)
729 if (recording_)
730 writeData(msgList.front());
731
732
733 #ifdef SICKLMS_SH_MEM
734 /// push data in shared memory
735 // First the scan info
736 SickLMS_shMem toWrite;
737 toWrite.time = msgToProcess.time;
738 toWrite.timerange = msgToProcess.timerange;
739 toWrite.scanInfo = msgToProcess.hScan;
740 shmem_->write(toWrite, sizeof(SickLMS_shMem));
741
742 // Then, the points
743 for (unsigned int i = 0 ; i < msgToProcess.hScan.numPoints ; ++i) {
744 shmem_->write((msg.body + i*sizeof(ScanPoint)), sizeof(ScanPoint));
745 }
746 #endif
747 }
748 else if (type == SICKLMS_SCANCONFIG_MESSAGE){
749 LOG_TRACE("Scan configuration message !");
750
751 // get the values as (unsigned or signed) integer
752 mainCfg.scaningFrequency = xstol(msgToProcess->splitMessage->at(2));
753 mainCfg.angleResolution = xstol(msgToProcess->splitMessage->at(4));
754 mainCfg.startAngle = xstol(msgToProcess->splitMessage->at(5));
755 mainCfg.stopAngle = xstol(msgToProcess->splitMessage->at(6));
756 LOG_TRACE("(Scan config) Frequency : "<<mainCfg.scaningFrequency<<", Resolution : "<<mainCfg.angleResolution);
757 LOG_TRACE("(Scan config) Start angle : "<<mainCfg.startAngle<<", Stop angle : "<<mainCfg.stopAngle);
758 }
759 }
760
761 delete msgToProcess->splitMessage;
762 }
763
764
765 // (malloced memory) raw data no longer needed
766 free(msgToProcess->body);
767
768
769 // removes the processed item of the list
770 msgList.pop_front();
771 }
772}
773
774} // namespace pacpus
Note: See TracBrowser for help on using the repository browser.