[37] | 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 |
|
---|
| 31 | using namespace std;
|
---|
| 32 |
|
---|
| 33 | // #define SICKLMS_SH_MEM
|
---|
| 34 |
|
---|
| 35 | namespace pacpus {
|
---|
| 36 |
|
---|
| 37 | static const int STX_CHAR = 0x02;
|
---|
| 38 | static const int ETX_CHAR = 0x03;
|
---|
| 39 |
|
---|
| 40 | DECLARE_STATIC_LOGGER("pacpus.base.SickLMSSensor");
|
---|
| 41 |
|
---|
| 42 | // Commands which can be taken into account
|
---|
| 43 | static const std::string SICKLMS_SCANDATA_MESSAGE = "LMDscandata";
|
---|
| 44 | static const std::string SICKLMS_SCANCONFIG_MESSAGE = "LMPscancfg";
|
---|
| 45 | static const int MINIMUM_PARAMETERS_MSG = 6; // ScanConfig message is the smallest relevant message
|
---|
| 46 |
|
---|
| 47 | //////////////////////////////////////////////////////////////////////////
|
---|
| 48 | /// Constructor.
|
---|
| 49 | SickLMSSensor::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 |
|
---|
| 68 | SickLMSSensor::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.
|
---|
| 90 | SickLMSSensor::~SickLMSSensor()
|
---|
| 91 | {
|
---|
| 92 | LOG_TRACE("destructor ("<< this->name_ << ")");
|
---|
| 93 | delete S_socket;
|
---|
| 94 | }
|
---|
| 95 |
|
---|
| 96 | //////////////////////////////////////////////////////////////////////////
|
---|
| 97 | /// Called by the ComponentManager to start the component
|
---|
| 98 | void 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
|
---|
| 178 | void 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 | ///
|
---|
| 204 | void 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
|
---|
| 215 | void 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 | ///
|
---|
| 226 | int 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
|
---|
| 239 | int 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
|
---|
| 254 | void 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 | //////////////////////////////////////////////////////////////////////////
|
---|
| 269 | void 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 | */
|
---|
| 341 | int 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
|
---|
| 362 | long 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 |
|
---|
| 380 | int 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:
|
---|
| 591 | void 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.
|
---|
| 692 | void 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
|
---|