source: pacpussensors/trunk/Alasca/AlascaComponent.cpp@ 113

/*********************************************************************
//  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 <iostream>
#include <QTcpSocket>
#include <string>

using namespace std;

namespace pacpus {

DECLARE_STATIC_LOGGER("pacpus.base.AlascaComponent");

// Construct the factory
static ComponentFactory<AlascaComponent> 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;
        setRecording(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 (isRecording()) {
        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 (isRecording()) {
        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 (config.getProperty("recording") != QString::null)
        setRecording ( config.getProperty("recording").toInt() );
    if (config.getProperty("alascaIP") != QString::null)
        host_ = config.getProperty("alascaIP");
    if (config.getProperty("alascaPort") != QString::null)
        port_ = config.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<char*>(&(alascaData.point[i].scannerId)), sizeof(uint8_t));
        dataFile_.write(reinterpret_cast<char*>(&(alascaData.point[i].layerNumber)), sizeof(uint8_t));
        dataFile_.write(reinterpret_cast<char*>(&(alascaData.point[i].echoNumber)), sizeof(uint8_t));
        dataFile_.write(reinterpret_cast<char*>(&(alascaData.point[i].pointStatus)), sizeof(uint8_t));
        dataFile_.write(reinterpret_cast<char*>(&(alascaData.point[i].x)), sizeof(int16_t));
        dataFile_.write(reinterpret_cast<char*>(&(alascaData.point[i].y)), sizeof(int16_t));
        dataFile_.write(reinterpret_cast<char*>(&(alascaData.point[i].z)), sizeof(int16_t));
        dataFile_.write(reinterpret_cast<char*>(&(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<char*>(&(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 (isRecording()) writeData();
            // push data in shared memory
            //shmem_->write(&alascaData, sizeof(alascaData));
        }

        // removes the processed item of the list
        msgList.pop_front();
    }
}

} // namespace pacpus