LD_MRS.cpp

Go to the documentation of this file.

//
// LD_MRS.cpp
//
// Device class of a simple LD-MRS laserscanner interface.
//
//
// To configure the device, add the parameters in the constructor below.
//
// VERSION 1.1.0
//
// Version history:
// 1.0.0  , willhvo
//   - Initial version
//
// 1.1.0  , willhvo
//   - Added reading from file
//


//
#include "LD_MRS.hpp"
#include "../tools/errorhandler.hpp"

namespace devices
{

using namespace datatypes;

LDMRS::LDMRS(Manager* manager)
        : m_manager(manager)
        , m_sopas(NULL)
        , m_lux(NULL)
{
        setDevicetype(Sourcetype_LDMRS);
        m_beVerbose = false;
        
        printInfoMessage("LDMRS::LDMRS: Starting constructor.", m_beVerbose);
        
        m_lux = NULL;
        
        m_weWantScanData = true;        // Enable or disable scan data reception via the MRS (=non-SOPAS) interface.
        m_weWantObjectData = false;     // Enable or disable object data reception via the MRS (=non-SOPAS) interface.
        m_weWantFieldData = false;      // Enable or disable protection field and eval case data reception via the SOPAS interface.
        m_weWantScanDataFromSopas = false;      // Enable or disable scan data reception via the SOPAS interface.

//      m_inputFileName = "/home/willhvo/Temp/raw_ldmrs_log.bin";               // File name of input file. If this name is given, connects to file instead of TCP.
        m_inputFileName = "";
        m_ipAddress = "192.168.0.1";    // , "Network IP address where the device is located.", "0.0.0.0");
        m_luxPortNumber = 12002;        // Network port on which the legacy (non-sopas) device is contacted (for scans and objects), typically 12002
        m_SopasPortNumber = 2111;       // Network port on which the device is contacted, typically 2111 or 2112.

        // parameters for LuxBase/Scans
        m_scanFrequency = 12.5;                         // Scan frequency (12.5, 25 or 50 Hz)
        m_scanStartAngle = 45.0 * deg2rad;      // Start angle of the scan.
        m_scanEndAngle = -45.0 * deg2rad;       // End angle of the scan.
        m_readOnlyMode = false;         // Forbids parameter writes to the scanner if true.

//      m_scanAngleRes = 0.25;
//      m_skipNumScans = 0;             // Use this value if the sensor scan frequency is too high and if scans should be skipped.

        m_offsetX = 0.0;        // x-coordinate of sensor mounting position in the vehicle coordinate system.
        m_offsetY = 0.0;        // y-coordinate of sensor mounting position in the vehicle coordinate system
        m_offsetZ = 0.0;        // z-coordinate of sensor mounting position in the vehicle coordinate system
        m_yawAngle = 0.0;       // Yaw angle of sensor mounting position in the vehicle coordinate system ("left and right")
        m_pitchAngle = 0.0;     // Pitch angle of sensor mounting position in the vehicle coordinate system ("up and down")
        m_rollAngle = 0.0;      // Roll angle of sensor mounting position in the vehicle coordinate system ("to the sides")

        printInfoMessage("LDMRS::LDMRS: Constructor done.", m_beVerbose);
}



LDMRS::~LDMRS()
{
        printInfoMessage("LDMRS::~LDMRS: Starting destructor.", m_beVerbose);
        
        if (isRunning())
        {
                stop();
        }
        
        printInfoMessage("LDMRS::~LDMRS: Destructor done.", m_beVerbose);
}


//
// Initialising.
//
// It would be nicer if this function would be called with all the device parameters,
// instead of setting them as module variables...
//
bool LDMRS::init()
{
        printInfoMessage("LDMRS::init: Called.", m_beVerbose);

        std::string longName = "LD-MRS";
        bool result = true;

//      m_beVerbose = false;
        m_isRunning = false;
        
        // Altes Objekt loeschen
        if (m_lux != NULL)
        {
                m_lux->stop();
                delete m_lux;
                m_lux = NULL;
        }

        // initializing LUX part
        printInfoMessage("LDMRS::init: Creating new LuxBase object.", m_beVerbose);
        m_lux = new LuxBase(m_manager,
                                                        getSourceId(),
                                                        longName,
                                                        m_ipAddress,
                                                        m_luxPortNumber,
                                                        m_scanFrequency,
                                                        m_scanStartAngle,
                                                        m_scanEndAngle,
                                                        m_offsetX,
                                                        m_offsetY,
                                                        m_offsetZ,
                                                        m_yawAngle,
                                                        m_pitchAngle,
                                                        m_rollAngle,
                                                        m_beVerbose,
                                                        m_inputFileName
                                        );
        
        printInfoMessage("LDMRS::init: Initializing the LuxBase object.", m_beVerbose);
        if (m_inputFileName == "")
        {
                // TCP
                result = m_lux->initTcp(LDMRS::disconnectFunctionS, (void*)this);
        }
        else
        {
                // File
                result = m_lux->initFile(LDMRS::disconnectFunctionS, (void*)this);
        }

        if (result == true)
        {
                // Success
                printInfoMessage("LDMRS::init: LuxBase was successfully initialized.", m_beVerbose);
        }
        else
        {
                // Error
                printError("LDMRS::init: Failed to initialize LuxBase - Device init failed, aborting.");
                return false;
        }
        
        // Set the current time
        Time t = Time::now();
        if (m_inputFileName == "")
        {
                // TCP
                result = m_lux->cmd_setNtpTimestamp((UINT32)(t.seconds() + Time::secondsFrom1900to1970), 0);
                if (result == true)
                {
                        // Success
                        printInfoMessage("LDMRS::init: NTP timestamp was successfully set.", m_beVerbose);
                }
                else
                {
                        // Error
                        printError("LDMRS::init: Failed to set NTP timestamp - Device init failed, aborting.");
                        return false;
                }
        }


        return true;
}


//
// Static-Einstiegspunkt fuer die DisconnectFunction.
//
void LDMRS::disconnectFunctionS(void* obj)
{
        printInfoMessage("LDMRS::disconnectFunctionS: Called.", true);

        if (obj != NULL)
        {
                ((LDMRS*)obj)->disconnectFunction();
        }
        
//      SystemStateWatchdog::setNotConnected();

        printInfoMessage("LDMRS::disconnectFunctionS: Done.", true);
}

//
//
//
void LDMRS::disconnectFunction()
{
        printInfoMessage("LDMRS::disconnectFunction: LDMRS " + getDeviceName() + " was disconnected.", m_beVerbose);

//      SystemStateWatchdog::setNotConnected();
}


//
// Schliesse alle Schnittstellen.
//
void LDMRS::shutdown()
{
        printInfoMessage("LDMRS::shutdown: called. Shutting down the LDMRS" + getDeviceName() + ".", m_beVerbose);

        if (m_lux->isRunning())
        {
                stop();
        }
}

//
// Write a field to the sensor
//
bool LDMRS::writeField(UINT16 fieldNum, const FieldParameter& para)
{
        bool result;
        bool beVerboseHere = m_beVerbose;
        beVerboseHere = true;
        
        printInfoMessage("LDMRS::writeField: Called. Logging in now.", beVerboseHere);
        result = m_sopas->action_login();
        
        if (result == true)
        {
                printInfoMessage("LDMRS::writeField: Login was successful, writing field now.", beVerboseHere);
                result = m_sopas->action_writeField(fieldNum, para);
        }
        
        if (result == true)
        {
                printInfoMessage("LDMRS::writeField: Field was written, logging out now.", beVerboseHere);
                result = m_sopas->action_logout();
        }
        
        printInfoMessage("LDMRS::writeField: All done, leaving.", beVerboseHere);
        return result;
}

//
// Write an EvalCase configuration to the sensor.
// This is a structure with all (up to 16) eval cases.
//
bool LDMRS::writeEvalCases(const EvalCases& evalCases)
{
        bool result;
        bool beVerboseHere = m_beVerbose;
        beVerboseHere = true;
        
        printInfoMessage("LDMRS::writeEvalCases: Called. Logging in now.", beVerboseHere);
        result = m_sopas->action_login();
        
        if (result == true)
        {
                printInfoMessage("LDMRS::writeEvalCases: Login was successful, writing eval cases now.", beVerboseHere);
                result = m_sopas->action_writeEvalCases(evalCases);
        }
        
        if (result == true)
        {
                printInfoMessage("LDMRS::writeEvalCases: Eval cases were written, logging out now.", beVerboseHere);
                result = m_sopas->action_logout();
        }
        
        printInfoMessage("LDMRS::writeEvalCases: All done, leaving.", beVerboseHere);
        return result;
}

//
// Stores the SOPAS config data (fields and eval cases) permanently.
//
bool LDMRS::flashSopasConfig()
{
        bool result;
        bool beVerboseHere = m_beVerbose;
        beVerboseHere = true;
        
        printInfoMessage("LDMRS::flashSopasConfig: Called.", beVerboseHere);

        result = m_sopas->action_flashFieldParameters();
        if (result == true)
        {
                printInfoMessage("LDMRS::flashSopasConfig: Configuration was saved.", beVerboseHere);
        }
        else
        {
                // Failure
                printError("LDMRS::flashSopasConfig: Failed to save configuration!");
        }
        
        printInfoMessage("LDMRS::writeField: All done, leaving.", beVerboseHere);
        return result;
}


//
// Sets the MRS-internal clock to the given time.
//
bool LDMRS::setNtpTime(UINT32 seconds, UINT32 fractionalSec)
{
        bool result;
        bool beVerboseHere = m_beVerbose;
        beVerboseHere = true;
        
        printInfoMessage("LDMRS::setNtpTime: Called.", beVerboseHere);
        
        if (m_lux == NULL)
        {
                printError("LDMRS::setNtpTime: No LUX-Base object available, aborting!");
                return false;
        }

        result = m_lux->cmd_setNtpTimestamp(seconds, fractionalSec);
        if (result == true)
        {
                printInfoMessage("LDMRS::setNtpTime: Timestamp was set.", beVerboseHere);
        }
        else
        {
                // Failure
                printError("LDMRS::setNtpTime: Failed to set timestamp!");
        }
        
        printInfoMessage("LDMRS::setNtpTime: All done, leaving.", beVerboseHere);
        return result;
}

bool LDMRS::setScanAngles(double startAngle, double endAngle)
{
        bool result;
        bool beVerboseHere = m_beVerbose;

        printInfoMessage("LDMRS::setScanAngles: Called.", beVerboseHere);

        if (m_lux == NULL)
        {
                printError("LDMRS::setScanAngles: No LUX-Base object available, aborting!");
                return false;
        }

        result = m_lux->cmd_setScanAngles(startAngle, endAngle);
        if (result == true)
        {
                printInfoMessage("LDMRS::setScanAngles: scan angles were set.", beVerboseHere);
        }
        else
        {
                // Failure
                printError("LDMRS::setScanAngles: Failed to set scan angles!");
        }

        printInfoMessage("LDMRS::setScanAngles: All done, leaving.", beVerboseHere);
        return result;
}

bool LDMRS::setSyncAngleOffset(double syncAngle)
{
        bool result;
        bool beVerboseHere = m_beVerbose;

        printInfoMessage("LDMRS::setSyncAngleOffset: Called.", beVerboseHere);

        if (m_lux == NULL)
        {
                printError("LDMRS::setSyncAngleOffset: No LUX-Base object available, aborting!");
                return false;
        }

        result = m_lux->cmd_setSyncAngleOffset(syncAngle);
        if (result == true)
        {
                printInfoMessage("LDMRS::setSyncAngleOffset: sync angle offset was set.", beVerboseHere);
        }
        else
        {
                // Failure
                printError("LDMRS::setSyncAngleOffset: Failed to set sync angle offset!");
        }

        printInfoMessage("LDMRS::setSyncAngleOffset: All done, leaving.", beVerboseHere);
        return result;
}

bool LDMRS::setScanFrequency(double scanFreq)
{
        bool result;
        bool beVerboseHere = m_beVerbose;

        printInfoMessage("LDMRS::setScanFrequency: Called.", beVerboseHere);

        if (m_lux == NULL)
        {
                printError("LDMRS::setScanFrequency: No LUX-Base object available, aborting!");
                return false;
        }

        result = m_lux->cmd_setScanFrequency(scanFreq);
        if (result == true)
        {
                printInfoMessage("LDMRS::setScanFrequency: scan frequency was set.", beVerboseHere);
        }
        else
        {
                // Failure
                printError("LDMRS::setScanFrequency: Failed to set scan frequency!");
        }

        printInfoMessage("LDMRS::setScanFrequency: All done, leaving.", beVerboseHere);
        return result;
}

std::string LDMRS::getIpAddress()
{
        return m_ipAddress;
}

void LDMRS::setIpAddress(std::string ipAdress)
{
        m_ipAddress = ipAdress;
}

void LDMRS::setWeWantObjectData(bool weWantObjectData) {
        m_weWantObjectData = weWantObjectData;
}

std::string LDMRS::getSerialNumber()
{
        if (m_lux == NULL)
        {
                return "(none)";
        } else {
                return m_lux->getSerialNumber();
        }
}

std::string LDMRS::getFirmwareVersion()
{
        if (m_lux == NULL)
        {
                return "(none)";
        } else {
                return m_lux->getFirmwareVersion();
        }
}

std::string LDMRS::getFPGAVersion()
{
        if (m_lux == NULL)
        {
                return "(none)";
        } else {
                return m_lux->getFPGAVersion();
        }
}

double LDMRS::getTemperature()
{
        if (m_lux == NULL)
        {
                return ::NaN_double;
        } else {
                return m_lux->getTemperature();
        }
}

//
// Starte das Einlesen von Daten (Scans, Objekte, Schutzfelder, ...)
//
bool LDMRS::run()
{
        if (m_lux == NULL)
        {
                printError("LDMRS::run: called, but pointer to internal data handler is NULL. Was init() called? Aborting!");
                return false;
        }
        
        //
        // LUX-Part
        //
        printInfoMessage("LDMRS::run: Called. Run the LDMRS" + getDeviceName() + ".", m_beVerbose);

        // This will cause that we get scan and/or obect data via lux protocol
        bool luxSuccess = m_lux->run(m_weWantScanData, m_weWantObjectData);
        if (luxSuccess == true)
        {
                // Success
                printInfoMessage("LDMRS::run: Device " + getDeviceName() + " is running.", m_beVerbose);
                m_lux->setOnScanReceiveCallbackFunction(LDMRS::onScanReceivedS, (void*)this);
        }
        else
        {
                // Fail
                printError("LDMRS::run: Failed to run device " + getDeviceName() + ", aborting!");
                return false;
        }

        //
        // SOPAS part
        //
        if ((m_inputFileName == "") &&
                ((m_weWantFieldData == true) ||
                 (m_weWantScanDataFromSopas == true)))
        {
                // TCP only
                bool sopasSuccess = true;
                printInfoMessage("LDMRS::run: Now starting SOPAS part (eval cases) of device " + getDeviceName() + ".", m_beVerbose);

                // Initialize Sopas part
                m_sopas = new LdmrsSopasLayer(m_manager,
                                                                        getSourceId(),
                                                                        m_ipAddress,
                                                                        m_SopasPortNumber,
                                                                        m_weWantFieldData,
                                                                        m_weWantScanDataFromSopas,
                                                                        m_readOnlyMode);
                sopasSuccess = m_sopas->init(LDMRS::disconnectFunctionS, (void*)this);

                if (sopasSuccess == false)
                {
                        printError("LDMRS::run: Failed to initialize SOPAS part of device " + getDeviceName() + ", aborting!");
                        stop();
                        return false;
                }

                // Success, now run the device
                printInfoMessage("LDMRS::run: SOPAS part is initialized, now running it.", m_beVerbose);
                sopasSuccess = m_sopas->run();
                if (sopasSuccess == false)
                {
                        printError("LDMRS::run: Failed to run SOPAS part of device " + getDeviceName() + ", aborting!");
                        stop();
                        return false;
                }
                        
                printInfoMessage("LDMRS::run: SOPAS part is running, device " + getDeviceName() + " is running, all done.", m_beVerbose);
        }
        
        return true;
}


//
// Stop data reception, but do not shut down the connection.
//
bool LDMRS::stop()
{
        printInfoMessage("LDMRS::stop: Called. Stopping the LDMRS" + getDeviceName() + ".", m_beVerbose);
        
        bool luxSuccess = true;
        bool sopasSuccess = true;
        
        if (m_sopas != NULL)
        {
                if (m_sopas->isFieldDataSubscribed())
                {
                        sopasSuccess = m_sopas->action_unSubscribeEvalCaseResults();
                }
        }

        if (m_lux != NULL)
        {
                luxSuccess = m_lux->stop();
        }
        
        return (luxSuccess && sopasSuccess);
}


bool LDMRS::isRunning()
{
        if (m_lux == NULL) // no m_lux object yet? Is not running.
        {
                return false;
        }
        bool luxSuccess = m_lux->isRunning();
        //bool sopasSuccess = m_sopas.isRunning();
        return (luxSuccess);    //  && sopasSuccess
}

//
// Statischer Einstiegspunkt
//
void LDMRS::onScanReceivedS(void* obj)
{
        if (obj != NULL)
        {
                ((LDMRS*)obj)->onScanReceived();
        }
}

// after / before every scan we have to post the current sensor state as SensorInfo Structure
void LDMRS::onScanReceived()
{
//      printInfoMessage("LDMRS::onScanReceived: Called.", m_beVerbose);
        
//      SensorStateInfo sensorInfo = m_sopas.getSensorStateInfo();
//      getSerializer().notifyMessageHandlers(sensorInfo, getDeviceID());
}

bool LDMRS::getParameter(MrsParameterId id, UINT32* value)
{
        return m_lux->cmd_getParameter(id, value);
}

bool LDMRS::setParameter(MrsParameterId id, UINT32 value)
{
        return m_lux->cmd_setParameter(id, value);
}


} // namespace devices