test.cpp

Go to the documentation of this file.

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          http://www.mrpt.org/                          |
   |                                                                        |
   | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file     |
   | See: http://www.mrpt.org/Authors - All rights reserved.                |
   | Released under BSD License. See details in http://www.mrpt.org/License |
   +------------------------------------------------------------------------+ */

#include <mrpt/obs/CObservationGasSensors.h>
#include <mrpt/config/CConfigFile.h>
#include <mrpt/io/CFileGZInputStream.h>
#include <mrpt/io/CFileGZOutputStream.h>
#include <mrpt/math/ops_containers.h>
#include <mrpt/system/filesystem.h>
#include <mrpt/serialization/CArchive.h>
#include <iostream>

using namespace mrpt;
using namespace mrpt::system;
using namespace mrpt::obs;
using namespace std;
using namespace mrpt::config;
using namespace mrpt::io;
using namespace mrpt::math;
using namespace mrpt::serialization;

int main(int argc, char** argv)
{
    // Variables
    string rawlog_file, sensorLabel;
    int enoseID, sensorType, indexMonitoredSensor,
        delay_value;  // decimate_value, winNoise_size
    mrpt::obs::CObservationGasSensors::CMOSmodel MOSmodel;
    bool have_estimation, apply_delay;

    // Load configuration:
    if (mrpt::system::fileExists("./CONFIG_MOXmodel.ini"))
    {
        cout << "Using configuration from './CONFIG_MOXmodel.ini'" << endl;
        CConfigFile conf("./CONFIG_MOXmodel.ini");

        rawlog_file = conf.read_string("", "rawlog_file", "", true);
        sensorLabel = conf.read_string("", "sensorLabel", "Full_MCEnose", true);
        enoseID = conf.read_int("", "enoseID", 0, true);
        std::string sensorType_str =
            conf.read_string("", "sensorType", "-1", true);
        stringstream convert(sensorType_str);
        convert >> std::hex >> sensorType;

        // Delays
        apply_delay = conf.read_bool("", "apply_delay", false, true);
        delay_value = conf.read_int("", "delay_value", 0, true);

        // MOX model parameters
        MOSmodel.a_rise = conf.read_float("", "a_rise", 0.0, true);
        MOSmodel.b_rise = conf.read_float("", "b_rise", 0.0, true);
        MOSmodel.a_decay = conf.read_float("", "a_decay", 0.0, true);
        MOSmodel.b_decay = conf.read_float("", "b_decay", 0.0, true);
        MOSmodel.winNoise_size = conf.read_int("", "winNoise_size", 0, true);
        MOSmodel.decimate_value = conf.read_int("", "decimate_value", 0, true);
        // save_maplog = conf.read_bool("","save_maplog",false,true);

        indexMonitoredSensor = -1;
    }
    else
    {
        cout << "Configuration file (ini) cannot be found\n" << endl;
        // If you are in VisualStudio, assure that the working directory in the
        // project properties is correctly set
        return -1;
    }

    // Open Rawlogs
    cout << "Processing Rawlog " << rawlog_file << endl;
    cout << "Obtaining MOXmodel from " << sensorLabel << "(" << enoseID
         << ") - sensor " << sensorType << endl;
    CFileGZInputStream file_input;
    CFileGZOutputStream file_output;

    file_input.open(rawlog_file);
    file_output.open("MOX_model_output.rawlog");

    if (!file_input.fileOpenCorrectly() || !file_output.fileOpenCorrectly())
        cout << "Error opening rawlog file" << endl;

    // Process rawlog
    bool read = true;
    while (read)
    {
        try
        {
            CSerializable::Ptr o;
            archiveFrom(file_input) >> o;

            if (o)  // ASSERT_(o);
            {
                if (IS_CLASS(o, CObservationGasSensors))
                {
                    CObservationGasSensors::Ptr obs =
                        std::dynamic_pointer_cast<CObservationGasSensors>(o);

                    // Correct delay on gas readings
                    if (apply_delay)
                        obs->timestamp = obs->timestamp -
                                         std::chrono::milliseconds(delay_value);

                    if (obs->sensorLabel == sensorLabel)
                    {
                        //------------------------------------------------------
                        // Get reading from CH_i for gas distribution estimation
                        //------------------------------------------------------
                        float raw_reading;

                        if (sensorType == 0)
                        {  // compute the mean
                            raw_reading = math::mean(
                                obs->m_readings[enoseID].readingsVoltage);
                        }
                        else
                        {
                            // Get the reading of the specified sensorID
                            if (indexMonitoredSensor == -1)
                            {
                                // First reading, get the index according to
                                // sensorID
                                for (indexMonitoredSensor = 0;
                                     indexMonitoredSensor <
                                     (int)obs->m_readings[enoseID]
                                         .sensorTypes.size();
                                     indexMonitoredSensor++)
                                {
                                    if (obs->m_readings[enoseID].sensorTypes.at(
                                            indexMonitoredSensor) ==
                                        std::vector<int>::value_type(
                                            sensorType))
                                        break;
                                }
                            }

                            if (indexMonitoredSensor <
                                (int)obs->m_readings[enoseID]
                                    .sensorTypes.size())
                            {
                                raw_reading =
                                    obs->m_readings[enoseID].readingsVoltage.at(
                                        indexMonitoredSensor);
                            }
                            else  // Sensor especified not found, compute
                            // default mean value
                            {
                                cout << "sensorType not found. Computing the "
                                        "mean value"
                                     << endl;
                                raw_reading = math::mean(
                                    obs->m_readings[enoseID].readingsVoltage);
                            }
                        }

                        // Obtain MOX model output
                        TPose3D MOXmodel_pose =
                            obs->m_readings[enoseID].eNosePoseOnTheRobot;
                        float MOXmodel_estimation = raw_reading;
                        mrpt::system::TTimeStamp MOXmodel_timestamp =
                            obs->timestamp;

                        have_estimation =
                            MOSmodel.get_GasDistribution_estimation(
                                MOXmodel_estimation, MOXmodel_timestamp);

                        if (have_estimation)
                        {
                            // Save as new obs
                            mrpt::obs::CObservationGasSensors::TObservationENose
                                gd_est;
                            gd_est.hasTemperature = false;
                            gd_est.temperature = 0.0;
                            gd_est.isActive = false;
                            gd_est.sensorTypes.push_back(
                                0x0001);  // indicates that is a MOXmodel output
                            gd_est.readingsVoltage.push_back(
                                MOXmodel_estimation);
                            gd_est.eNosePoseOnTheRobot = MOXmodel_pose;

                            mrpt::obs::CObservationGasSensors::Ptr obs_GDM =
                                mrpt::make_aligned_shared<
                                    CObservationGasSensors>();
                            obs_GDM->sensorLabel = "GDM";
                            // modify timestamp to deal with the delay of the
                            // model
                            obs_GDM->timestamp = MOXmodel_timestamp;
                            obs_GDM->m_readings.push_back(gd_est);

                            archiveFrom(file_output) << obs_GDM;
                        }
                    }
                }

                // Save current sensor obs to the new Rawlog file
                auto arch = archiveFrom(file_output);
                arch << o;
            }
        }
        catch (exception& e)
        {
            cout << "Exception: " << e.what() << endl;
            file_input.close();
            file_output.close();
            read = false;
        }
    }

    return 0;
}