source: pacpussensors/trunk/StdDbtPlayerComponents/DbtPlyAlascaManager.cpp

// *********************************************************************
//  created:    2007/11/13 - 16:49
//  filename:   DbtPlyAlascaManager.cpp
//
//  author:     Gerald Dherbomez
//              Copyright Heudiasyc UMR UTC/CNRS 6599
//
//  version:    $Id: DbtPlyAlascaManager.cpp 1199 2012-08-01 08:51:24Z kurdejma $
//
//  purpose:
// *********************************************************************

#include "DbtPlyAlascaManager.h"

#include <boost/assert.hpp>
#include <iostream>
#include <string>
#include <math.h>

#include "kernel/Log.h"
#include "kernel/InputOutputInterface.h"
#include "PacpusTools/ShMem.h"

#define CENTIMETER_TO_METER 100

namespace pacpus {

using namespace std;

DECLARE_STATIC_LOGGER("pacpus.base.DbtPlyAlascaManager");

/// Construction de la fabrique de composant DbtPlyAlascaManager
static ComponentFactory<DbtPlyAlascaManager> sFactory("DbtPlyAlascaManager");

//static const char * kAlaskaMemoryName = "alasca";// deprecated

//////////////////////////////////////////////////////////////////////////
/// Constructor.
DbtPlyAlascaManager::DbtPlyAlascaManager(QString name)
    : DbtPlyFileManager(name)
{
    LOG_TRACE("constructor(" << name << ")");

    // mShMem = new ShMem(kAlaskaMemoryName, sizeof(ScanAlascaData)); // deprecated
}

//////////////////////////////////////////////////////////////////////////
/// Destructor.
DbtPlyAlascaManager::~DbtPlyAlascaManager()
{
    LOG_TRACE("destructor");
    //delete mShMem; // deprecated
}

//////////////////////////////////////////////////////////////////////////
/// Configure the component.
ComponentBase::COMPONENT_CONFIGURATION DbtPlyAlascaManager::configureComponent(XmlComponentConfig config)
{
    DbtPlyFileManager::configureComponent(config);
    mDataFilename = config.getProperty("binFile");

    return ComponentBase::CONFIGURED_OK;
}

void DbtPlyAlascaManager::addInputs()
{
    // empty: no inputs
}

void DbtPlyAlascaManager::addOutputs()
{
    addOutput<LidarScan, DbtPlyAlascaManager>("scan");
    addOutput<ScanAlascaData, DbtPlyAlascaManager>("raw");
}

//////////////////////////////////////////////////////////////////////////
/// Starts the component.
void DbtPlyAlascaManager::startActivity()
{
    mDataFilename = mEngine->getDataDir() + mDataFilename;

    mDataFile.open(mDataFilename.toLatin1().data(),std::ios_base::in|std::ios_base::binary);
    if (!mDataFile) {
        LOG_ERROR("cannot open file '" << mDataFilename << "'");
        return;
    }
    DbtPlyFileManager::startActivity();
}

//////////////////////////////////////////////////////////////////////////
/// Stops the component.
void DbtPlyAlascaManager::stopActivity()
{
    DbtPlyFileManager::stopActivity();
    mDataFile.close();
}

//////////////////////////////////////////////////////////////////////////
/// processData
void DbtPlyAlascaManager::processData(road_time_t t, road_timerange_t tr, void * buffer)
{
    if (!buffer) {
        LOG_DEBUG("no data available: NULL buffer");
        return;
    }

    LOG_TRACE("sizeof(AlascaXT) = " << sizeof(AlascaXT));
    BOOST_ASSERT(24 == sizeof(AlascaXT));
    AlascaXT * alascaXT = static_cast<AlascaXT *>(buffer);

    // copy the values contained in the dbt file
    mAlascaData.startAngle = alascaXT->startAngle;
    mAlascaData.endAngle = alascaXT->endAngle;
    mAlascaData.nbPoint = alascaXT->nbPoint;
    mAlascaData.scannertype = alascaXT->scannertype;
    mAlascaData.timeStart = alascaXT->timeStart;
    mAlascaData.time = t;
    mAlascaData.timerange = tr;

    // set the get pointer to the correct place
    mDataFile.seekg(alascaXT->dataPos);

    // then copy the data contained in the binary file
    for (size_t i = 0; i < mAlascaData.nbPoint; ++i) {
        mDataFile.read(reinterpret_cast<char *>(&(mAlascaData.point[i].scannerId)), sizeof(uint8_t));
        mDataFile.read(reinterpret_cast<char *>(&(mAlascaData.point[i].layerNumber)), sizeof(uint8_t));
        mDataFile.read(reinterpret_cast<char *>(&(mAlascaData.point[i].echoNumber)), sizeof(uint8_t));
        mDataFile.read(reinterpret_cast<char *>(&(mAlascaData.point[i].pointStatus)), sizeof(uint8_t));
        mDataFile.read(reinterpret_cast<char *>(&(mAlascaData.point[i].x)), sizeof(int16_t));
        mDataFile.read(reinterpret_cast<char *>(&(mAlascaData.point[i].y)), sizeof(int16_t));
        mDataFile.read(reinterpret_cast<char *>(&(mAlascaData.point[i].z)), sizeof(int16_t));
        mDataFile.read(reinterpret_cast<char *>(&(mAlascaData.point[i].width)), sizeof(uint16_t));
    }

    lidarScan = LidarScan(4);
    int layer;

    lidarScan.nbPoint = mAlascaData.nbPoint;
    lidarScan.time = mAlascaData.time;
    lidarScan.timerange = mAlascaData.timerange;
    lidarScan.scannerId;
    lidarScan.scannerType = mAlascaData.scannertype;
    lidarScan.nbLayer = 4;

    for (size_t i = 0; i < mAlascaData.nbPoint; ++i) {
         LidarPoint point;

         layer = mAlascaData.point[i].layerNumber;
         point.echo = mAlascaData.point[i].echoNumber;

         point.x = (float)mAlascaData.point[i].x / CENTIMETER_TO_METER;
         point.y = (float)mAlascaData.point[i].y / CENTIMETER_TO_METER;
         point.z = (float)mAlascaData.point[i].z / CENTIMETER_TO_METER;
         point.intensity = mAlascaData.point[i].width;
         point.d = (sqrt(point.x*point.x+point.y*point.y+point.z*point.z) * CENTIMETER_TO_METER);

         lidarScan.layers[layer].nbPoint++;
         lidarScan.layers[layer].points.append(point);
    }

    // verify that the last value is the UTC magic word
    int32_t utcMagicWord = 0;
    mDataFile.read(reinterpret_cast<char *>(&(utcMagicWord)), sizeof(utcMagicWord));
    if (UTC_MAGIC_WORD != utcMagicWord) {
        LOG_WARN("corrupted data, do not use them!");
        LOG_DEBUG("wrong magic word: EXPECTED=" << UTC_MAGIC_WORD << ", ACTUAL=" << utcMagicWord);
    } else {
        LOG_TRACE("writing scan");
        //mShMem->write(&mAlascaData, sizeof(ScanAlascaData));

        OutputInterface<LidarScan, DbtPlyAlascaManager> * scanOutput = getTypedOutput<LidarScan, DbtPlyAlascaManager>("scan");
        if (scanOutput && scanOutput->hasConnection()) {
            scanOutput->send(lidarScan, road_time(), /*timerange=*/50000); // FIXME: magic numbers?
        }
        OutputInterface<ScanAlascaData, DbtPlyAlascaManager> * rawOutput = getTypedOutput<ScanAlascaData, DbtPlyAlascaManager>("raw");
        if (rawOutput && rawOutput->hasConnection()) {
            rawOutput->send(mAlascaData, road_time(), /*timerange=*/50000);
        }
    }

    if (mVerbose) {
        cout << "[ALASCA]:\t"
             << "#points=" << mAlascaData.nbPoint << "\t"
             << "timeStart=" << mAlascaData.timeStart << endl
                ;
    }
    if (mVerbose >= 2)  {
        cout << "[ALASCA]:\t"
             << "scannertype=" << (int) mAlascaData.scannertype << "\t"
             << "startAngle=" << mAlascaData.startAngle << "\t"
             << "endAngle=" << mAlascaData.endAngle << endl
                ;
    }
}

//////////////////////////////////////////////////////////////////////////
/// Displays the graphical user interface (GUI)
void DbtPlyAlascaManager::displayUI()
{
    LOG_WARN("GUI not implemented");

    // TODO
}

} // namespace pacpus