Main MRPT website > C++ reference for MRPT 1.5.7
CEnoseModular.cpp
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2017, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +---------------------------------------------------------------------------+ */
9 
10 #include "hwdrivers-precomp.h" // Precompiled headers
11 
12 #include <mrpt/system/os.h>
14 #include <mrpt/utils/CMessage.h>
15 
16 using namespace mrpt::utils;
17 using namespace mrpt::math;
18 using namespace mrpt::obs;
19 using namespace mrpt::poses;
20 using namespace mrpt::hwdrivers;
21 using namespace std;
22 
24 
25 /*-------------------------------------------------------------
26  CEnoseModular
27 -------------------------------------------------------------*/
29  m_usbSerialNumber ("ENOSE002"),
30  m_COM_port (),
31  m_COM_baud (115200),
32  m_stream_FTDI (NULL),
33  m_stream_SERIAL (NULL)
34 {
35  m_sensorLabel = "EnoseModular";
36  first_reading = true;
37 }
38 
39 CEnoseModular::~CEnoseModular( )
40 {
41  delete_safe(m_stream_FTDI);
42  delete_safe(m_stream_SERIAL);
43 }
44 
45 /*-------------------------------------------------------------
46  loadConfig_sensorSpecific
47 -------------------------------------------------------------*/
48 void CEnoseModular::loadConfig_sensorSpecific(
49  const mrpt::utils::CConfigFileBase &configSource,
50  const std::string &iniSection )
51 {
53 
54  m_usbSerialNumber = configSource.read_string(iniSection, "USB_serialname","",false);
55 
56 #ifdef MRPT_OS_WINDOWS
57  m_COM_port = configSource.read_string(iniSection, "COM_port_WIN","COM1");
58 #else
59  m_COM_port = configSource.read_string(iniSection, "COM_port_LIN",m_COM_port);
60 #endif
61  m_COM_baud = configSource.read_uint64_t(iniSection, "COM_baudRate",m_COM_baud);
62 
63 
64  MRPT_END
65 
66 }
67 
68 
69 /*-------------------------------------------------------------
70  checkConnectionAndConnect
71 -------------------------------------------------------------*/
72 CStream *CEnoseModular::checkConnectionAndConnect()
73 {
74  // Make sure one of the two possible pipes is open:
75  if (!m_stream_FTDI && !m_stream_SERIAL)
76  {
77  if (!m_COM_port.empty())
78  m_stream_SERIAL = new CSerialPort();
79  else m_stream_FTDI = new CInterfaceFTDI();
80  }
81 
82 
83  if (m_stream_FTDI)
84  { // FTDI pipe ==================
85  if (m_stream_FTDI->isOpen())
86  return m_stream_FTDI;
87  try
88  {
89  m_stream_FTDI->OpenBySerialNumber( m_usbSerialNumber );
91  m_stream_FTDI->Purge();
93  m_stream_FTDI->SetLatencyTimer(1);
94  m_stream_FTDI->SetTimeouts(10,100);
95  return m_stream_FTDI;
96  }
97  catch(...)
98  { // Error opening device:
99  m_stream_FTDI->Close();
100  return NULL;
101  }
102  }
103  else
104  { // Serial pipe ==================
105  ASSERT_(m_stream_SERIAL)
106  if (m_stream_SERIAL->isOpen())
107  return m_stream_SERIAL;
108  try
109  {
110  m_stream_SERIAL->open(m_COM_port);
111  m_stream_SERIAL->setConfig(m_COM_baud);
112  //m_stream_SERIAL->setTimeouts(25,1,100, 1,20);
113  m_stream_SERIAL->setTimeouts(50,1,100, 1,20);
115  m_stream_SERIAL->purgeBuffers();
117  return m_stream_SERIAL;
118  }
119  catch(...)
120  { // Error opening device:
121  m_stream_SERIAL->close();
122  return NULL;
123  }
124  }
125 }
126 
127 
128 /*-------------------------------------------------------------
129  getObservation
130 -------------------------------------------------------------*/
132 {
133  try
134  {
135  // Connected?
136  CStream *comms = checkConnectionAndConnect();
137 
138  if (!comms)
139  {
140  cout << "ERORR: Problem connecting to Device." << endl;
141  return false;
142  }
143 
144 
146  obs.m_readings.clear();
147 
148  //---------------------------- Enose Modular FRAME --------------------------------------------------
149  // Wait for e-nose frame: <0x69><0x91><lenght><body><0x96> "Bytes"
150  // Where <body> = <temp>[<SensorID_H><SensorID_L><Sensor_Value>] x N_senosrs
151  // Modular-nose provides a 4B+body frame lenght
152 
154  bool time_out = false;
156  double time_out_val = 1; //seconds
157 
158  while (!comms->receiveMessage( msg ) && !time_out)
159  {
161  time_out = true;
162  }
163 
164  if (time_out)
165  {
166  cout << "[CEnoseModular - getObservation] measurement Timed-Out" << endl;
167  return false;
168  }
169 
170  if (msg.content.size()>0)
171  {
172  // Each sensor reading is composed of 3 Bytes [<SensorID_H><SensorID_L><Sensor_Value>]
173  ASSERT_( (msg.content.size()-1)%3 == 0 )
174  size_t numSensors = (msg.content.size()-1)/3;
175 
176  // Prepare the Enose observation
177  newRead.sensorTypes.clear();
178  newRead.readingsVoltage.clear();
179  newRead.hasTemperature = true;
180  newRead.isActive = true;
181 
182  // Do we have the sensor position?
183  if (enose_poses_x.size() != 0)
184  {
185  newRead.eNosePoseOnTheRobot = CPose3D(
186  enose_poses_x[0],
187  enose_poses_y[0],
188  enose_poses_z[0],
189  enose_poses_yaw[0],
190  enose_poses_pitch[0],
191  enose_poses_roll[0]);
192  }
193  else
194  newRead.eNosePoseOnTheRobot = CPose3D(0,0,0);
195 
196  // Get Temperature (degrees C)
197  newRead.temperature = msg.content[0]*1.65214 - 277.74648;
198 
199  //process all sensors
200  for (size_t idx=0 ; idx<numSensors ; idx++)
201  {
202  // Copy ID (2 BYTES) To integer
203  int sensorType_temp = 0;
204  //memcpy( &sensorType, &msg.content[idx*3+1], 2*sizeof(msg.content[0]) );
205  memcpy( &sensorType_temp, &msg.content[idx*3+1], sizeof(msg.content[0]) );
206  int sensorType = sensorType_temp << (8) ;
207  memcpy( &sensorType, &msg.content[idx*3+2], sizeof(msg.content[0]) );
208 
209  //Add sensor Type (ID)
210  newRead.sensorTypes.push_back(sensorType);
211 
212  // Transform from ADC value[8bits] to [0-0.6] volt range:
213  newRead.readingsVoltage.push_back( ( msg.content[idx*3+3] * 0.6f) / 255.0f );
214 
215  }
216 
217  //Purge buffers
218  purgeBuffers();
219 
220  // Add data to observation:
221  obs.m_readings.push_back( newRead );
222  obs.sensorLabel = m_sensorLabel;
224  return !obs.m_readings.empty(); // Done OK!
225 
226  }
227  else
228  {
229  cout << "Message was empty" << endl;
230  return false;
231  }
232 
233  }
234  catch(exception &e)
235  {
236  cerr << "[CEnoseModular::getObservation] Returning false due to exception: " << endl;
237  cerr << e.what() << endl;
238  return false;
239  }
240  catch(...)
241  {
242  return false;
243  }
244 }
245 
246 
247 
248 /*-------------------------------------------------------------
249  doProcess
250 -------------------------------------------------------------*/
251 /** This method should be called periodically (at least at 1Hz to capture ALL the real-time data)
252 * It is thread safe, i.e. you can call this from one thread, then to other methods from other threads.
253 */
254 void CEnoseModular::doProcess()
255 {
256  CObservationGasSensorsPtr obs= CObservationGasSensors::Create();
257 
258  if (getObservation(*obs))
259  {
260  m_state = ssWorking;
261  appendObservation( obs );
262  }
263  else
264  {
265  m_state = ssError;
266  cout << "No observation received from the USB board!" << endl;
267  // THROW_EXCEPTION("No observation received from the USB board!");
268  }
269 }
270 
271 /*-------------------------------------------------------------
272  purgeBuffers
273 -------------------------------------------------------------*/
274 void CEnoseModular::purgeBuffers()
275 {
276  if (!checkConnectionAndConnect())
277  return;
278 
279  if (m_stream_FTDI)
280  { // FTDI pipe
281  m_stream_FTDI->Purge();
282  }
283  else
284  { //Serial pipe
285  m_stream_SERIAL->purgeBuffers();
286  }
287 }
288 
uint64_t TTimeStamp
A system independent time type, it holds the the number of 100-nanosecond intervals since January 1...
Definition: datetime.h:30
void BASE_IMPEXP memcpy(void *dest, size_t destSize, const void *src, size_t copyCount) MRPT_NO_THROWS
An OS and compiler independent version of "memcpy".
Definition: os.cpp:358
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: zip.h:16
OBSERVATION_T::Ptr getObservation(mrpt::obs::CSensoryFramePtr &observations, mrpt::obs::CObservationPtr &observation, bool priority_to_sf=true)
Given an mrpt::obs::CSensoryFrame and a mrpt::obs::CObservation pointer if a OBSERVATION_T type obser...
Definition: obs_utils.h:32
A communications serial port built as an implementation of a utils::CStream.
Definition: CSerialPort.h:43
mrpt::system::TTimeStamp BASE_IMPEXP getCurrentTime()
Returns the current (UTC) system time.
Definition: datetime.cpp:71
float temperature
Sensed temperature in Celcius (valid if hasTemperature=true only)
Contains classes for various device interfaces.
vector_int sensorTypes
The kind of sensors in the array (size of "sensorTypes" is the same that the size of "readingsVoltage...
std::string read_string(const std::string &section, const std::string &name, const std::string &defaultValue, bool failIfNotFound=false) const
STL namespace.
mrpt::system::TTimeStamp BASE_IMPEXP getCurrentLocalTime()
Returns the current (local) time.
Definition: datetime.cpp:173
This class allows loading and storing values and vectors of different types from a configuration text...
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:38
This base provides a set of functions for maths stuff.
Definition: CArrayNumeric.h:19
void delete_safe(T *&ptr)
Calls "delete" to free an object only if the pointer is not NULL, then set the pointer to NULL...
Definition: bits.h:196
math::TPose3D eNosePoseOnTheRobot
The pose of the sensors on the robot.
#define MRPT_END
void BASE_IMPEXP sleep(int time_ms) MRPT_NO_THROWS
An OS-independent method for sending the current thread to "sleep" for a given period of time...
Definition: threads.cpp:57
bool hasTemperature
Must be true for "temperature" to contain a valid measurement.
This namespace contains representation of robot actions and observations.
Declares a class derived from "CObservation" that represents a set of readings from gas sensors...
GLsizei const GLchar ** string
Definition: glext.h:3919
#define IMPLEMENTS_GENERIC_SENSOR(class_name, NameSpace)
This must be inserted in all CGenericSensor classes implementation files:
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CPoint.h:17
#define MRPT_START
std::string sensorLabel
An arbitrary label that can be used to identify the sensor.
bool isActive
True if the input to this chamber/enose is poluted air, False if clean air.
mrpt::system::TTimeStamp timestamp
The associated UTC time-stamp. Where available, this should contain the accurate satellite-based time...
bool receiveMessage(utils::CMessage &msg)
Tries to receive a message from the device.
Definition: CStream.cpp:590
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
std::vector< TObservationENose > m_readings
One entry per e-nose on the robot.
#define ASSERT_(f)
uint64_t read_uint64_t(const std::string &section, const std::string &name, uint64_t defaultValue, bool failIfNotFound=false) const
std::vector< uint8_t > content
The contents of the message (memory is automatically handled by the std::vector object) ...
Definition: CMessage.h:33
double BASE_IMPEXP timeDifference(const mrpt::system::TTimeStamp t_first, const mrpt::system::TTimeStamp t_later)
Returns the time difference from t1 to t2 (positive if t2 is posterior to t1), in seconds...
Definition: datetime.cpp:205
std::vector< float > readingsVoltage
The set of readings (in volts) from the array of sensors (size of "sensorTypes" is the same that the ...
A definition of a CStream actually representing a USB connection to a FTDI chip.
A class that contain generic messages, that can be sent and received from a "CClientTCPSocket" object...
Definition: CMessage.h:29
A class for interfacing an e-NoseModular via a FTDI USB link.
Definition: CEnoseModular.h:52



Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019