/********************************************************************* // created: 2007/11/07 - 12:08 // filename: AlascaComponent.cpp // // author: Gerald Dherbomez & Sergio Rodriguez // Copyright Heudiasyc UMR UTC/CNRS 6599 // // version: $Id: $ // // purpose: The acquisition component of the Ibeo Alasca sensor // *********************************************************************/ #include "AlascaComponent.h" //#include "AlascaDataGenerator.h" #include "AlascaSocket.h" #include "Pacpus/kernel/ComponentFactory.h" #include "Pacpus/kernel/DbiteException.h" #include "Pacpus/kernel/DbiteFileTypes.h" #include "Pacpus/kernel/Log.h" #include "Pacpus/PacpusTools/ShMem.h" #include #include #include using namespace std; namespace pacpus { DECLARE_STATIC_LOGGER("pacpus.base.AlascaComponent"); // Construct the factory static ComponentFactory sFactory("AlascaComponent"); static const string kAlaskaMemoryName = "alasca"; static const string kAlaskaDbtFileName = "alasca.dbt"; static const string kAlaskaUtcFileName = "alasca_data.utc"; static const long MagicWord = 0x55544300; static const int ALASCA_OBJECTDATA_TYPE = 1; static const int ALASCA_SCANDATA_TYPE = 15; ////////////////////////////////////////////////////////////////////////// /// Constructor. AlascaComponent::AlascaComponent(QString name) : ComponentBase(name) { LOG_TRACE("constructor(" << name << ")"); A_socket = new AlascaSocket(this); pendingBytes.time = 0; pendingBytes.previousData = false; recording = false; // default values host_ = "172.17.130.205"; port_ = 12000; } ////////////////////////////////////////////////////////////////////////// /// Destructor. AlascaComponent::~AlascaComponent() { LOG_TRACE("destructor"); delete A_socket; } ////////////////////////////////////////////////////////////////////////// /// Called by the ComponentManager to start the component void AlascaComponent::startActivity() { // uncomment the following line to use the Alasca Data generator and comment the second //generator = new AlascaDataGenerator(this); A_socket->connectToServer(host_, port_); if (recording) { try { dbtFile_.open(kAlaskaDbtFileName, WriteMode, ALASCA_XT, sizeof(AlascaXT)); } catch (DbiteException & e) { cerr << "error opening dbt file: " << e.what() << endl; return; } // FIXME: use ofstream // open the file with C function to be sure that it will exist FILE * stream; if (NULL == (stream = fopen(kAlaskaUtcFileName.c_str(), "a+"))) { LOG_FATAL("cannot open file '" << kAlaskaUtcFileName.c_str() << "'"); ::exit(-1); } else { fclose(stream); } dataFile_.open(kAlaskaUtcFileName.c_str(), ios_base::out|ios_base::binary|ios_base::app); if (!dataFile_) { LOG_FATAL("cannot open file '" << kAlaskaUtcFileName.c_str() << "'"); ::exit(-1); } } shmem_ = new ShMem(kAlaskaMemoryName.c_str(), sizeof(ScanAlascaData)); } ////////////////////////////////////////////////////////////////////////// /// Called by the ComponentManager to stop the component void AlascaComponent::stopActivity() { A_socket->closeSocket(); if (recording) { dbtFile_.close(); dataFile_.close(); } delete shmem_; // delete generator; } ////////////////////////////////////////////////////////////////////////// /// Called by the ComponentManager to pass the XML parameters to the /// component ComponentBase::COMPONENT_CONFIGURATION AlascaComponent::configureComponent(XmlComponentConfig config) { if (param.getProperty("recording") != QString::null) recording = param.getProperty("recording").toInt(); if (param.getProperty("alascaIP") != QString::null) host_ = param.getProperty("alascaIP"); if (param.getProperty("alascaPort") != QString::null) port_ = param.getProperty("alascaPort").toInt(); return ComponentBase::CONFIGURED_OK; } ////////////////////////////////////////////////////////////////////////// /// look for the position of the magic word in the packet. /// Return it if found else return -1 long AlascaComponent::findMagicWord(const char * message, const unsigned length) { if (length < 4) { return -1; } unsigned long i = 0; unsigned long magicWord=0x00000000; magicWord = ( (*(message+i+3)) & 0x000000FF ) + ( ((*(message+i+2))<<8) & 0x0000FF00 ) + ( ((*(message+i+1))<<16) & 0x00FF0000 ) + ( ((*(message+i))<<24) & 0xFF000000 ); while (magicWord !=0xAFFEC0C0) { ++i; magicWord = ( (*(message+i+3)) & 0x000000FF ) + ( ((*(message+i+2))<<8) & 0x0000FF00 ) + ( ((*(message+i+1))<<16) & 0x00FF0000 ) + ( ((*(message+i))<<24) & 0xFF000000 ); if (i == length) { return -1; } continue; } return i; } ////////////////////////////////////////////////////////////////////////// /// Return the size of the message contained in the packet /// You have to provide the position of the magic word 0xAFFEC0C0 in the packet long AlascaComponent::getMessageSize(const char * message, const unsigned length, const long magicWordIndex) { // we need at least 8 bytes if (length < 8) { return 0; } return ((*(message+magicWordIndex+7))&0x000000FF) + ((*(message+magicWordIndex+6)<<8)&0x0000FF00) + ((*(message+magicWordIndex+5)<<16)&0x00FF0000) + ((*(message+magicWordIndex+4)<<24)&0xFF000000) + 16; } ////////////////////////////////////////////////////////////////////////// /// @param size the total size of the message including the 2 headers and the body /// @param length the number of available bytes in the packet bool AlascaComponent::isMessageComplete(const unsigned length, const long size) { if (size <= length) { return true; } return false; } ////////////////////////////////////////////////////////////////////////// /// fill the scan header of the message msg /// warning: the body field of the message have to be completed before void AlascaComponent::fillScanHeader( Message &msg ) { msg.hScan.version = *(msg.body+16); msg.hScan.scannerType = *(msg.body+17); msg.hScan.ecuId = *(msg.body+18); // byte 19 is a padding byte msg.hScan.timeStamp = ((*(msg.body+23))&0x000000FF)+ ((*(msg.body+22)<<8)&0x0000FF00)+ ((*(msg.body+21)<<16)&0x00FF0000)+ ((*(msg.body+20)<<24)&0xFF000000); msg.hScan.startAngle = ((*(msg.body+25))&0x00FF)+ ((*(msg.body+24)<<8)&0xFF00); msg.hScan.endAngle = ((*(msg.body+27))&0x00FF)+ ((*(msg.body+26)<<8)&0xFF00); msg.hScan.scanCounter = ((*(msg.body+29))&0x00FF)+ ((*(msg.body+28)<<8)&0xFF00); msg.hScan.numPoints = ((*(msg.body+31))&0x00FF)+ ((*(msg.body+30)<<8)&0xFF00); } ////////////////////////////////////////////////////////////////////////// /// fill the message header of the message msg /// warning: the body field of the message have to be completed before void AlascaComponent::fillMessageHeader(Message &msg) { msg.hMsg.magicWord = ((*(msg.body+3))&0x000000FF) + ((*(msg.body+2)<<8)&0x0000FF00) + ((*(msg.body+1)<<16)&0x00FF0000)+ ((*(msg.body)<<24)&0xFF000000); msg.hMsg.size = ((*(msg.body+7))&0x000000FF)+ ((*(msg.body+6)<<8)&0x0000FF00)+ ((*(msg.body+5)<<16)&0x00FF0000)+ ((*(msg.body+4)<<24)&0xFF000000); msg.hMsg.dataType = ((*(msg.body+11))&0x000000FF)+ ((*(msg.body+10)<<8)&0x0000FF00)+ ((*(msg.body+9)<<16)&0x00FF0000)+ ((*(msg.body+8)<<24)&0xFF000000); msg.hMsg.timestamp =((*(msg.body+15))&0x000000FF)+ ((*(msg.body+14)<<8)&0x0000FF00)+ ((*(msg.body+13)<<16)&0x00FF0000)+ ((*(msg.body+12)<<24)&0xFF000000); } ////////////////////////////////////////////////////////////////////////// /// this function is called when no complete message has been found /// we set the flag previousData to true for the next processing stage /// and we store the timestamp of the bytes acquisition void AlascaComponent::storePendingBytes(road_time_t time) { if (!pendingBytes.previousData) { pendingBytes.time = time; pendingBytes.previousData = true; } } ////////////////////////////////////////////////////////////////////////// /// Analyse the ethernet packet received from the Alasca and try to find a /// complete message (scan data message or object message) /// If a message has been found it is added at the end of the message list /// else the pending bytes are stored to be analyzed by further icoming data ////////////////////////////////////////////////////////////////////////// void AlascaComponent:: splitPacket(const char * packet, const int length, road_time_t time) { //unsigned long ptr = 0; long index = 0; long msgSize = 0; bool msgComplete = false; // we are working on the previous not decoded data + the actual incoming packet pendingBytes.data.append(packet,length); while (pendingBytes.data.size() > 0) { // we are looking for the MagicWord index = findMagicWord(pendingBytes.data.c_str() , pendingBytes.data.size() ); if (index == -1) { storePendingBytes(time); // exit the while loop break; } // we are looking for the size of the message (message header + scan header + body) msgSize = getMessageSize(pendingBytes.data.c_str() , pendingBytes.data.size() , index ); if (msgSize == 0) { storePendingBytes(time); // exit the while loop break; } // we are verifying if the message is complete msgComplete = isMessageComplete( pendingBytes.data.size() , msgSize ); if (msgComplete == false) { storePendingBytes(time); // exit the while loop break; } // we have a complete message available that we can add to the list Message msg; // we copy the bytes in the body message memcpy(msg.body, pendingBytes.data.c_str() + index, msgSize); // we set the timestamp of the message if (pendingBytes.previousData) { // the timestamp is the one of the previous packet msg.time = pendingBytes.time; pendingBytes.previousData = false; } else { // the timestamp is the one of the actual received packet msg.time = time; } // we add the message to the list msgList.push_back(msg); // and we suppress the processed bytes of the pending data // problème ici pendingBytes.data.erase(0, msgSize); } } ////////////////////////////////////////////////////////////////////////// /// Convert the integer value returned by the sensor in a cartesian coordinate /// return the value in centimeters /// see Manual_ALASCA.pdf p29/30, note 8 ////////////////////////////////////////////////////////////////////////// short AlascaComponent::din70000toCentimeters(short n) { // remark: short is included between -32768 and 32767 so if n>10000 or n<-10000 // the return value must be limited to these values and the max and min values // of n are obtained by these expressions: // 10 * n_max - 90000 <= 32767 so n_max <= 12276 // 10 * n_min + 90000 >= -32768 so n_min >= -12276 // the return value will be rounded to this value if n exceeds this limit short ret = 0; if (n>=32768) n = n - 65536; if ( (n>10000) && (n<=12276) ) ret = 10 * n - 90000; if ( n>12276 ) ret = 32767; if ( (n<-10000) && (n>=-12276) ) ret = 10 * n + 90000; if (n<-12276) ret = -32768; if ( (n >= -10000) && (n <= 10000) ) ret = n; return ret; } ////////////////////////////////////////////////////////////////////////// /// Convert the integer value returned by the sensor in a cartesian coordinate /// return the value in meters /// see Manual_ALASCA.pdf p29/30, note 8 ////////////////////////////////////////////////////////////////////////// float AlascaComponent::din70000toMeters(short n) { float ret = 0.0; if (n>=32768) n = n - 65536; if (n>10000) ret = n*0.1 - 900 ; if (n<-10000) ret = n*0.1 + 900; if ( (n>= -10000) && (n <= 10000) ) ret = n*0.01; return ret; } ////////////////////////////////////////////////////////////////////////// /// Process the message /// Fill the 2 headers (message and scan) and update the scan data /// process only ALASCA_SCANDATA_TYPE message unsigned long AlascaComponent::processMessage(Message &msg) { fillMessageHeader(msg); if (ALASCA_SCANDATA_TYPE == msg.hMsg.dataType) { fillScanHeader(msg); // fill the alascaData structure alascaData.nbPoint = msg.hScan.numPoints; alascaData.startAngle = msg.hScan.startAngle; alascaData.endAngle = msg.hScan.endAngle; alascaData.scannertype = msg.hScan.scannerType; alascaData.timeStart = msg.hScan.timeStamp; alascaData.time = msg.time; alascaData.timerange = msg.timerange; int index = 32; short value; for (int i = 0; i < msg.hScan.numPoints; ++i) { alascaData.point[i].scannerId = *(msg.body + index); alascaData.point[i].layerNumber = *(msg.body + index + 1); alascaData.point[i].echoNumber = *(msg.body + index + 2); alascaData.point[i].pointStatus = *(msg.body + index + 3); value = ((*(msg.body + index + 5))&(0x00FF))+ ((*(msg.body + index + 4)<<8)&(0xFF00)); alascaData.point[i].x = din70000toCentimeters(value); //alascaData.point[i].x = din70000toMeters(value); value = ((*(msg.body + index + 7))&(0x00FF))+ ( (*(msg.body + index + 6)<<8)&(0xFF00)); alascaData.point[i].y = din70000toCentimeters(value); //alascaData.point[i].y = din70000toMeters(value); value = ((*(msg.body + index + 9))&(0x00FF))+ ((*(msg.body + index + 8)<<8)&(0xFF00)); alascaData.point[i].z = din70000toCentimeters(value); //alascaData.point[i].z = din70000toMeters(value); alascaData.point[i].width = ((*(msg.body + index + 11))&(0x00FF))+ ((*(msg.body + index + 10)<<8)&(0xFF00)); index += 12; } // update the dbt structure dbtData_.endAngle = alascaData.endAngle; dbtData_.startAngle = alascaData.startAngle; dbtData_.scannertype = alascaData.scannertype; dbtData_.timeStart = alascaData.timeStart; dbtData_.nbPoint = alascaData.nbPoint; } return msg.hMsg.dataType; } ////////////////////////////////////////////////////////////////////////// /// write the data on the disk: /// - complete the dbt file with dbtData_ /// - complete the binary data file with the alascaData.point void AlascaComponent::writeData() { // get the absolute position of the put pointer dbtData_.dataPos = dataFile_.tellp(); // record the data in a dbt file. The dbt data is only the structure AlascaXT // scan data are recording in the alasca_data.utc file try { dbtFile_.writeRecord(alascaData.time, alascaData.timerange, (char *) &dbtData_, sizeof(dbtData_)); } catch (DbiteException & e) { cerr << "error writing data: " << e.what() << endl; return; } // record the scan data in a binary file alasca_data.utc with UTC\0 to separate the data for (unsigned int i = 0 ; i < alascaData.nbPoint ; ++i) { dataFile_.write(reinterpret_cast(&(alascaData.point[i].scannerId)), sizeof(uint8_t)); dataFile_.write(reinterpret_cast(&(alascaData.point[i].layerNumber)), sizeof(uint8_t)); dataFile_.write(reinterpret_cast(&(alascaData.point[i].echoNumber)), sizeof(uint8_t)); dataFile_.write(reinterpret_cast(&(alascaData.point[i].pointStatus)), sizeof(uint8_t)); dataFile_.write(reinterpret_cast(&(alascaData.point[i].x)), sizeof(int16_t)); dataFile_.write(reinterpret_cast(&(alascaData.point[i].y)), sizeof(int16_t)); dataFile_.write(reinterpret_cast(&(alascaData.point[i].z)), sizeof(int16_t)); dataFile_.write(reinterpret_cast(&(alascaData.point[i].width)), sizeof(uint16_t)); } // add a magic word to delimit the block of data int32_t utcMagicWord = UTC_MAGIC_WORD; dataFile_.write(reinterpret_cast(&(utcMagicWord)), sizeof(int32_t)); } ////////////////////////////////////////////////////////////////////////// /// Event call by the AlascaSocket class when new data has been arrived /// on the network. void AlascaComponent::customEvent(QEvent * e) { //int type = e->type(); AlascaFrame * frame = ((AlascaFrameEvent*)e)->frame; // we try to find some messages in the current packet + the pending bytes of the previous incoming data splitPacket(frame->msg, frame->size, frame->time); // we delete the heap variable delete frame; // we test if we have some messages to decode while ( !msgList.empty() ) { // get the first (the eldest) message and process it unsigned long type = processMessage(msgList.front()); if (type == ALASCA_SCANDATA_TYPE) { setState(ComponentBase::MONITOR_OK); // write data on the disk if (recording) writeData(); // push data in shared memory shmem_->write(&alascaData, sizeof(alascaData)); } // removes the processed item of the list msgList.pop_front(); } } } // namespace pacpus