Example: obs_mox_model_rawlog
C++ example source code:
/* +------------------------------------------------------------------------+ | Mobile Robot Programming Toolkit (MRPT) | | https://www.mrpt.org/ | | | | Copyright (c) 2005-2022, Individual contributors, see AUTHORS file | | See: https://www.mrpt.org/Authors - All rights reserved. | | Released under BSD License. See: https://www.mrpt.org/License | +------------------------------------------------------------------------+ */ #include <mrpt/config/CConfigFile.h> #include <mrpt/io/CFileGZInputStream.h> #include <mrpt/io/CFileGZOutputStream.h> #include <mrpt/math/ops_containers.h> #include <mrpt/obs/CObservationGasSensors.h> #include <mrpt/serialization/CArchive.h> #include <mrpt/system/filesystem.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; auto obs_GDM = CObservationGasSensors::Create(); 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; }