source: pacpussensors/trunk/StdDbtPlayerComponents/DbtPlySickLDMRSManager.cpp

// *********************************************************************
//  created:    1014/03/27 - 11:37
//  filename:   DbtPlySickLDMRSManager.cpp
//
//  authors:     Gerald Dherbomez, Cyril Fougeray    
//              Copyright Heudiasyc UMR UTC/CNRS 6599
// 
//  version:    $Id$
//
//  purpose:    
// *********************************************************************

#include "DbtPlySickLDMRSManager.h"

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

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


#define UTC_MAGIC_WORD  0x55544300


namespace pacpus {

using namespace std;

DECLARE_STATIC_LOGGER("pacpus.base.DbtPlySickLDMRSManager");

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

static const char * kSickMemoryName = "sickLDMRS";

/************************************************************************
 * Constructor
 ************************************************************************/
DbtPlySickLDMRSManager::DbtPlySickLDMRSManager(QString name)
    : DbtPlyFileManager(name)
{
    LOG_TRACE("constructor(" << name << ")");
}

/************************************************************************
 * Destructor
 ************************************************************************/
DbtPlySickLDMRSManager::~DbtPlySickLDMRSManager()
{
    LOG_TRACE("destructor");
}

/************************************************************************
 * Configuration of the component, called by the ComponentManager after
 * the construction of the object
 ************************************************************************/
ComponentBase::COMPONENT_CONFIGURATION DbtPlySickLDMRSManager::configureComponent(XmlComponentConfig config)
{
    DbtPlyFileManager::configureComponent(config);

    mDataFilename = config.getProperty("binFile");

    return ComponentBase::CONFIGURED_OK;
}

/************************************************************************
 * Start function, called by the ComponentManager when a start()
 * command is received
 ************************************************************************/
void DbtPlySickLDMRSManager::startActivity()
{
    LOG_TRACE("DbtPlySickLDMRSManager component is starting.");

    mDataFilename = mEngine->getDataDir() + mDataFilename;

    LOG_TRACE("Opening "<< mDataFilename);

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

    outScan = getTypedOutput<SickLDMRSScan, DbtPlySickLDMRSManager>("scan");

    DbtPlyFileManager::startActivity();
}

/************************************************************************
 * Stop function, called by the ComponentManager when a stop()
 * command is received
 ************************************************************************/
void DbtPlySickLDMRSManager::stopActivity()
{
    DbtPlyFileManager::stopActivity();
    mDataFile.close();
}


/************************************************************************
 * Called by the framework at initialization
 ************************************************************************/
void DbtPlySickLDMRSManager::addInputs()
{
  // uncomment to add an input
}


/************************************************************************
 * Called by the framework at initialization
 ************************************************************************/
void DbtPlySickLDMRSManager::addOutputs()
{
  // empty: no output
  addOutput<SickLDMRSScan, DbtPlySickLDMRSManager>("scan");
}


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

    LOG_TRACE("sizeof(sickLDMRS_dbt) = " << sizeof(SickLDMRS_dbt));
    // BOOST_ASSERT(88 == sizeof(SickLMS_dbt));
    SickLDMRS_dbt * sickLDMRS_dbt = static_cast<SickLDMRS_dbt *>(buffer);

    // // copy the values contained in the dbt file
    mScan.timeStartFromSensor = sickLDMRS_dbt->timeStartFromSensor;
    mScan.header = sickLDMRS_dbt->hScan;
    mScan.time = t;
    mScan.timerange = tr;
    mScan.points = QVector<ScanPoint>(mScan.header.numPoints);

    LOG_TRACE("Number of points " << mScan.header.numPoints);

    LOG_TRACE("Reading UTC file ... ");

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

    // then copy the data contained in the binary file
    for (unsigned int i = 0 ; i < mScan.header.numPoints ; ++i) {
        mDataFile.read(reinterpret_cast<char *>(&(mScan.points[i])), sizeof(ScanPoint));
    }

    // verify that the last value is the UTC magic word
    int32_t utcMagicWord = 0;
    mDataFile.read(reinterpret_cast<char *>(&(utcMagicWord)), sizeof(int32_t));
    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("sending scan ");
        checkedSend(outScan, mScan);
    }

    if (mVerbose) {
        cout << "[SICK LDMRS]:\t"
             << "numPoints=" << mScan.header.numPoints << "\t"
             << "time=" << t << endl
                ;
    }
    if (mVerbose >= 2)  {
        cout << "[SICK LDMRS]:\t"
             << "startAngle=" << mScan.header.startAngle << "\t"
             << "endAngle=" << mScan.header.endAngle << std::endl ;
    }
}

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

    // TODO
}

} // namespace pacpus