/* +---------------------------------------------------------------------------+
| The Mobile Robot Programming Toolkit (MRPT) C++ library |
| |
| http://www.mrpt.org/ |
| |
| Copyright (C) 2005-2011 University of Malaga |
| |
| This software was written by the Machine Perception and Intelligent |
| Robotics Lab, University of Malaga (Spain). |
| Contact: Jose-Luis Blanco <jlblanco@ctima.uma.es> |
| |
| This file is part of the MRPT project. |
| |
| MRPT is free software: you can redistribute it and/or modify |
| it under the terms of the GNU General Public License as published by |
| the Free Software Foundation, either version 3 of the License, or |
| (at your option) any later version. |
| |
| MRPT is distributed in the hope that it will be useful, |
| but WITHOUT ANY WARRANTY; without even the implied warranty of |
| MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
| GNU General Public License for more details. |
| |
| You should have received a copy of the GNU General Public License |
| along with MRPT. If not, see <http://www.gnu.org/licenses/>. |
| |
+---------------------------------------------------------------------------+ */
#include <mrpt/hwdrivers/CBoardENoses.h>
#include <mrpt/base.h>
#include <mrpt/slam.h>
#include <mrpt/gui.h>
using namespace mrpt;
using namespace mrpt::hwdrivers;
using namespace mrpt::system;
using namespace mrpt::slam;
using namespace mrpt::utils;
using namespace std;
using namespace mrpt::gui;
//Global VARs
bool firstTime =true;
std::map<int, string> colors;
std::map<int, CDisplayWindowPlots*> Win_map;
float Hbaselines[] ={3.5, 1.44, 0.877, 0.71, 0.20, 0.18};
float Vbaselines[] ={30,45,300};
std::map<float, std::vector<float> > Baseline_map;
std::map<float, std::vector<float> > Vertical_lines_map;
//FUNCTIONS
void myOnMenu(int menuID,float x,float y, void*param)
{
cout << "Menu: " << menuID << endl << " x=" << x << " y="<< y << endl;
}
/** Destroy the windows created.
*/
void clear_memory()
{
for( map< int, CDisplayWindowPlots* >::iterator iter = Win_map.begin(); iter != Win_map.end(); ++iter )
{
delete iter->second;
iter->second = NULL;
}
}
void add_VLines()
{
std::vector <float> Vline;
for (float inc=0; inc<=5; inc+=0.05)
{
Vline.push_back(inc);
//Vertical lines.
for (size_t i=0; i<sizeof(Vbaselines)/4; i++)
{
Vertical_lines_map[Vbaselines[i]].push_back(Vbaselines[i]);
//plot it
char sensor_name [50];
sprintf(sensor_name,"%f",Vbaselines[i]);
string name(sensor_name);
Win_map[1]->plot(Vertical_lines_map[Vbaselines[i]],Vline,"k.1", sensor_name);
}
}
}
/** Plot each sensor reading over time
*/
void plot_diff_windows(std::map<int, vector<float> > &sensors, bool together)
{
//Create windows if first time.
try
{
if (together) //All sensor on same window
{
if (firstTime)
{
//Create windows
firstTime=false;
Win_map[0] = new CDisplayWindowPlots("Temperature :");
Win_map[0]->resize(800,900);
Win_map[0]->axis(-15,1800,-5,50,false);
Win_map[0]->axis_equal(false);
Win_map[0]->setPos(0,0);
Win_map[1] = new CDisplayWindowPlots("Sensor Readings :");
Win_map[1]->resize(800,900);
Win_map[1]->axis(-5,300,-1,6,false);
Win_map[1]->axis_equal(false);
Win_map[1]->setPos(810,0);
//Create color palet
colors[0x2600]="r-3";
colors[0x2611]="b-3";
colors[0x2620]="m-3";
colors[0x2442]="g-3";
colors[0x6810]="k-3";
colors[0x5135]="r-2";
colors[0x5521]="b-2";
colors[0x5042]="m-2";
colors[0x3]="g.0";
colors[0x41]="g.0";
colors[0x549]="g.0";
colors[0x1]="g.0";
add_VLines();
}
// Plot
for( map<int, std::vector<float> >::iterator iter = sensors.begin(); iter != sensors.end(); ++iter )
{
if (iter->first == 0xFFFF){ //Temperature
Win_map[0]->plot(sensors[0x0000], iter->second,"b-3");
}
else if (iter->first != (0x0000) ){ //Is not Time
Win_map[1]->plot(sensors[0x0000], iter->second,colors[iter->first], format("%i",int(iter->first)) );
}
}
}
else //Eah sensor has his own window
{
if (firstTime)
{
firstTime=false;
int posx=0;
int posy=0;
for( map<int, std::vector<float> >::iterator iter = sensors.begin(); iter != sensors.end(); ++iter )
{
if (iter->first != (0x0000) ){ //Is not Time
Win_map[iter->first] = new CDisplayWindowPlots( format("Sensor: %i",int(iter->first)) );
Win_map[iter->first]->resize(800,100);
Win_map[iter->first]->axis(-1,240,-1,6,false);
Win_map[iter->first]->axis_equal(false);
Win_map[iter->first]->setPos(posx*810,posy*160);
if(posy==5){posx++; posy=0;}
else{posy++;}
}
}
}
for( map<int, std::vector<float> >::iterator iter = sensors.begin(); iter != sensors.end(); ++iter )
{
if (iter->first != (0x0000) ){ //Is not Time
Win_map[iter->first]->plot(sensors[0x0000], sensors[iter->first],"b-3");
}
}
}//End-if(together)
}
catch(std::exception &e)
{
cerr << e.what() << endl;
}
}
//ADD_BASELINE
/**
If configured, add to the plot window different lines as references.
*/
void add_baseline(std::vector<float> time)
{
try
{
//Horizontal lines.
for (size_t i=0; i<sizeof(Hbaselines)/4; i++)
{
Baseline_map[Hbaselines[i]].push_back(Hbaselines[i]);
//plot it
char sensor_name [10];
sprintf(sensor_name,"%f",Hbaselines[i]);
string name(sensor_name);
Win_map[1]->plot(time, Baseline_map[Hbaselines[i]], "k.1", sensor_name);
}
}
catch(std::exception &e)
{
cerr << e.what() << endl;
}
}
int main()
{
try
{
CBoardENoses eNoses;
std::string firmVers;
CObservationGasSensors obs;
FILE *f_log = os::fopen(format("./log_%s.txt", fileNameStripInvalidChars(mrpt::system::dateTimeLocalToString(now())).c_str() ),"wt");
TTimeStamp timStart = mrpt::system::getCurrentTime();
std::map<int, std::vector<float> > Sensor_array;
bool together = true; //True -> Shows all sensor on same window. False -> Each sensor has his own window
/*
// Load configuration:
ASSERT_( mrpt::system::fileExists("_CONFIG_eNoses.ini") );
CConfigFile conf("_CONFIG_eNoses.ini");
eNoses.loadConfig( conf, "eNoses" );
if (!eNoses.queryFirmwareVersion( firmVers ) )
{
printf("Error!!\n");
return -1;
}
else
std::cout << "FIRMWARE VERSION: " << firmVers << std::endl;
*/
while ( !mrpt::system::os::kbhit() )
{
if (! eNoses.getObservation( obs ) ) //NEW OBSERVATION
{
cout << "- Could not retrieve an observation from the eNoses..." << endl;
}
else
{
//Push_back timeStamp (seconds)
Sensor_array[0x0000].push_back( mrpt::system::timeDifference(timStart,obs.timestamp) );
if (f_log) fprintf(f_log,"%f ", mrpt::system::timeDifference(timStart,obs.timestamp) );
for (size_t i=0;i<obs.m_readings.size();i++)
{
//Push_back Temperature
Sensor_array[0xFFFF].push_back( obs.m_readings[i].temperature );
if (f_log) fprintf(f_log,"%f ", obs.m_readings[i].temperature );
//E-Nose Sensor's Data
for (size_t j=0;j<obs.m_readings[i].sensorTypes.size();j++)
{
if ( j<(obs.m_readings[i].sensorTypes.size() -1) ){ //Not the las item
if( (obs.m_readings[i].sensorTypes[j]==obs.m_readings[i].sensorTypes[j+1]) ){
Sensor_array[ obs.m_readings[i].sensorTypes[j] ].push_back(obs.m_readings[i].readingsVoltage[j+1]-obs.m_readings[i].readingsVoltage[j]);
if (f_log) fprintf(f_log,"%f ",obs.m_readings[i].readingsVoltage[j+1]-obs.m_readings[i].readingsVoltage[j] );
j++; //skip the next sensor as it has been used already.
}else{
Sensor_array[ obs.m_readings[i].sensorTypes[j] ].push_back( obs.m_readings[i].readingsVoltage[j] );
if (f_log) fprintf(f_log,"%f ",obs.m_readings[i].readingsVoltage[j]); //To file
}
}else{
Sensor_array[ obs.m_readings[i].sensorTypes[j] ].push_back( obs.m_readings[i].readingsVoltage[j] );
if (f_log) fprintf(f_log,"%f ",obs.m_readings[i].readingsVoltage[j]); //To file
}
}
}
if (f_log) fprintf(f_log,"\n");
//Plotting information
plot_diff_windows(Sensor_array,together);
//Generate the Baseline
add_baseline(Sensor_array[0x0000]);
} //end-if getObs
mrpt::system::sleep(40);
}//end-While
clear_memory();
if (f_log) os::fclose(f_log);
}
catch(std::exception &e)
{
cerr << e.what() << endl;
return -1;
}
return 0;
}
