Main MRPT website > C++ reference for MRPT 1.5.7
CBoardENoses.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 #include <mrpt/math/ops_vectors.h>
16 
17 using namespace mrpt::utils;
18 using namespace mrpt::math;
19 using namespace mrpt::obs;
20 using namespace mrpt::poses;
21 using namespace mrpt::hwdrivers;
22 using namespace std;
23 
25 
26 /*-------------------------------------------------------------
27  CBoardENoses
28 -------------------------------------------------------------*/
30  m_usbSerialNumber ("ENOSE001"),
31  m_COM_port (),
32  m_COM_baud (115200),
33  m_stream_FTDI (NULL),
34  m_stream_SERIAL (NULL)
35 {
36  m_sensorLabel = "ENOSE";
37  first_reading = true;
38 }
39 
40 CBoardENoses::~CBoardENoses( )
41 {
42  delete_safe(m_stream_FTDI);
43  delete_safe(m_stream_SERIAL);
44 }
45 
46 /*-------------------------------------------------------------
47  loadConfig_sensorSpecific
48 -------------------------------------------------------------*/
49 void CBoardENoses::loadConfig_sensorSpecific(
50  const mrpt::utils::CConfigFileBase &configSource,
51  const std::string &iniSection )
52 {
54 
55  m_usbSerialNumber = configSource.read_string(iniSection, "USB_serialname","",false);
56 
57 #ifdef MRPT_OS_WINDOWS
58  m_COM_port = configSource.read_string(iniSection, "COM_port_WIN",m_COM_port);
59 #else
60  m_COM_port = configSource.read_string(iniSection, "COM_port_LIN",m_COM_port);
61 #endif
62  m_COM_baud = configSource.read_uint64_t(iniSection, "COM_baudRate",m_COM_baud);
63 
64  configSource.read_vector( iniSection, "enose_poses_x", vector<float>(0), enose_poses_x, true);
65  configSource.read_vector( iniSection, "enose_poses_y", vector<float>(0), enose_poses_y, true);
66  configSource.read_vector( iniSection, "enose_poses_z", vector<float>(0), enose_poses_z, true);
67 
68  configSource.read_vector( iniSection, "enose_poses_yaw", vector<float>(0), enose_poses_yaw, true);
69  configSource.read_vector( iniSection, "enose_poses_pitch", vector<float>(0), enose_poses_pitch, true);
70  configSource.read_vector( iniSection, "enose_poses_roll", vector<float>(0), enose_poses_roll, true);
71 
72  ASSERT_( enose_poses_x.size() == enose_poses_y.size() );
73  ASSERT_( enose_poses_x.size() == enose_poses_z.size() );
74  ASSERT_( enose_poses_x.size() == enose_poses_yaw.size() );
75  ASSERT_( enose_poses_x.size() == enose_poses_pitch.size() );
76  ASSERT_( enose_poses_x.size() == enose_poses_roll.size() );
77 
78  // Pass angles to radians:
79  enose_poses_yaw *= M_PIf / 180.0f;
80  enose_poses_pitch *= M_PIf / 180.0f;
81  enose_poses_roll *= M_PIf / 180.0f;
82 
83  MRPT_END
84 
85 }
86 
87 /*-------------------------------------------------------------
88  queryFirmwareVersion
89 -------------------------------------------------------------*/
90 bool CBoardENoses::queryFirmwareVersion( string &out_firmwareVersion )
91 {
92  try
93  {
94  mrpt::utils::CMessage msg,msgRx;
95 
96  // Try to connect to the device:
97  CStream *comms = checkConnectionAndConnect();
98  if (!comms)
99  return false;
100 
101  msg.type = 0x10;
102  comms->sendMessage(msg);
103 
104  if (comms->receiveMessage(msgRx) )
105  {
106  msgRx.getContentAsString( out_firmwareVersion );
107  return true;
108  }
109  else
110  return false;
111  }
112  catch(...)
113  {
114  // Close everything and start again:
115  delete_safe(m_stream_SERIAL);
116  delete_safe(m_stream_FTDI);
117  return false;
118  }
119 }
120 
121 
122 /*-------------------------------------------------------------
123  checkConnectionAndConnect
124 -------------------------------------------------------------*/
125 CStream *CBoardENoses::checkConnectionAndConnect()
126 {
127  // Make sure one of the two possible pipes is open:
128  if (!m_stream_FTDI && !m_stream_SERIAL)
129  {
130  if (!m_COM_port.empty())
131  m_stream_SERIAL = new CSerialPort();
132  else m_stream_FTDI = new CInterfaceFTDI();
133  }
134 
135 
136  if (m_stream_FTDI)
137  { // FTDI pipe ==================
138  if (m_stream_FTDI->isOpen())
139  return m_stream_FTDI;
140  try
141  {
142  m_stream_FTDI->OpenBySerialNumber( m_usbSerialNumber );
144  m_stream_FTDI->Purge();
146  m_stream_FTDI->SetLatencyTimer(1);
147  m_stream_FTDI->SetTimeouts(10,100);
148  return m_stream_FTDI;
149  }
150  catch(...)
151  { // Error opening device:
152  m_stream_FTDI->Close();
153  return NULL;
154  }
155  }
156  else
157  { // Serial pipe ==================
158  ASSERT_(m_stream_SERIAL)
159  if (m_stream_SERIAL->isOpen())
160  return m_stream_SERIAL;
161  try
162  {
163  m_stream_SERIAL->open(m_COM_port);
164  m_stream_SERIAL->setConfig(m_COM_baud);
166  m_stream_SERIAL->purgeBuffers();
168  //m_stream_SERIAL->setTimeouts(25,1,100, 1,20);
169  m_stream_SERIAL->setTimeouts(50,1,100, 1,20);
170  return m_stream_SERIAL;
171  }
172  catch(...)
173  { // Error opening device:
174  m_stream_SERIAL->close();
175  return NULL;
176  }
177  }
178 }
179 
180 
181 /*-------------------------------------------------------------
182  getObservation
183 -------------------------------------------------------------*/
185 {
186  try
187  {
188  // Connected?
189  CStream *comms = checkConnectionAndConnect();
190 
191  if (!comms)
192  return false;
193 
194  utils::CMessage msg;
196 
197  obs.m_readings.clear();
198 
199  //// Send request:
200  //msg.type = 0x11;
201  //msg.content.clear();
202  //comms->sendMessage( msg );
203 
204  //----------------------------MCE-nose FRAME--------------------------------------------------
205  // Wait for e-nose frame: <0x69><0x91><lenght><body><0x96> "Bytes"
206  // Where <body> = [Numchamber, Activechamber, N sensors*M chambers*2, 2 timestamp] of uint16_t
207  // MCE-nose provides a 136B body lenght which makes 140B total frame lenght
208 
209  if (!comms->receiveMessage( msg ))
210  {
211  return false;
212  }
213 
214  //m_state = ssWorking;
215 
216  // Copy to "uint16_t":
217  ASSERT_((msg.content.size() % 2)==0);
218 
219  vector<uint16_t> readings( msg.content.size() / 2 ); // divide by 2 to pass from byte to word. 136B/2 = 68 Words
220 
221  if (msg.content.size()>0)
222  {
223  // Copy to a vector of 16bit integers:
224  memcpy( &readings[0],&msg.content[0],msg.content.size() * sizeof(msg.content[0]) );
225 
226  //HEADER Frame [ NÂș of chambers/enoses (16b) , Active Chamber (16b)]
227  size_t NumberOfChambers = (size_t)readings[0];
228  size_t ActiveChamber = (size_t)readings[1];
229 
230  // Sensors readings info
231  ASSERT_( ((readings.size() - 4) % NumberOfChambers)==0 );
232  size_t wordsPereNose = (readings.size() - 4) / NumberOfChambers;
233 
234 
235  // Process each chamber
236  for (size_t i=0; i<NumberOfChambers; i++)
237  {
238  // ----------------------------------------------------------------------
239  // Each "i" comprises a complete Enose reading: Gas sensors + temperature
240  // ----------------------------------------------------------------------
241 
242  // Do we have the sensor position?
243  if (i<enose_poses_x.size())
244  {
245  newRead.eNosePoseOnTheRobot = CPose3D(
246  enose_poses_x[i],
247  enose_poses_y[i],
248  enose_poses_z[i],
249  enose_poses_yaw[i],
250  enose_poses_pitch[i],
251  enose_poses_roll[i]);
252  }
253  else
254  newRead.eNosePoseOnTheRobot = CPose3D(0,0,0);
255 
256  // Process the sensor codes:
257  newRead.sensorTypes.clear();
258  newRead.readingsVoltage.clear();
259  newRead.hasTemperature = false;
260  newRead.isActive = false;
261 
262  // check if active chamber
263  if (i == (ActiveChamber))
264  newRead.isActive = true;
265 
266  //process each sensor on this chamber "i"
267  for (size_t idx=0 ; idx<wordsPereNose/2 ; idx++)
268  {
269  if ( readings[i*wordsPereNose + 2*idx + 2] != 0x0000 ) //not empty slot
270  {
271  // Is temperature?
272  if (readings[i*wordsPereNose + 2*idx + 2] == 0xFFFF)
273  {
274  newRead.hasTemperature = true;
275  newRead.temperature = ((int16_t)readings[i*wordsPereNose + 2*idx + 3]) / 32.0f;
276  }
277  else //Is a gas sensors
278  {
279  // It is not a null code: There is a valid measure:
280  newRead.sensorTypes.push_back( readings[i*wordsPereNose + 2*idx + 2] );
281 
282  // Pass from ADC value[12bits] to [0-2.5] volt range:
283  newRead.readingsVoltage.push_back( ( readings[i*wordsPereNose + 2*idx + 3] * 5.0f) / 4096.0f );
284  }
285  }
286  } // end for each sensor on this eNose
287 
288  // Add to observations:
289  if ( !newRead.sensorTypes.empty() )
290  obs.m_readings.push_back( newRead );
291 
292  } // end for each i'th eNose
293 
294  obs.sensorLabel = m_sensorLabel;
295 
296  // Set Timestamp
297  uint16_t* p =
298  (uint16_t*)&readings[readings.size() - 2]; // Get readings time
299  // from frame
300  // (always last 2
301  // words)
302  obs.timestamp =
303  mrpt::system::secondsToTimestamp(((double)*p) / 1000);
304 
305  if (first_reading)
306  {
307  initial_timestamp = mrpt::system::getCurrentTime() - obs.timestamp;
308  first_reading = false;
309  }
310  obs.timestamp = obs.timestamp + initial_timestamp;
311 
312  } // end if message has data
313 
314 
315  //CONTROL
316  bool correct = true;
317 
318  if (obs.m_readings.size() != 4)
319  correct = false;
320  else
321  {
322  for (size_t ch=0; ch<obs.m_readings.size(); ch++)
323  {
324  if( (obs.m_readings[ch].sensorTypes.size() != 7) || (obs.m_readings[ch].readingsVoltage.size() != 7) )
325  correct = false;
326  else
327  {
328 
329  }
330  }
331  }
332 
333  if (!correct)
334  printf("Error en la observacion"); //For debug
335 
336 
337  return !obs.m_readings.empty(); // Done OK!
338  }
339  catch(exception &e)
340  {
341  cerr << "[CBoardENoses::getObservation] Returning false due to exception: " << endl;
342  cerr << e.what() << endl;
343  return false;
344  }
345  catch(...)
346  {
347  return false;
348  }
349 }
350 
351 
352 
353 /*-------------------------------------------------------------
354  doProcess
355 -------------------------------------------------------------*/
356 /** This method should be called periodically (at least at 1Hz to capture ALL the real-time data)
357 * It is thread safe, i.e. you can call this from one thread, then to other methods from other threads.
358 */
359 void CBoardENoses::doProcess()
360 {
361  CObservationGasSensorsPtr obs= CObservationGasSensors::Create();
362 
363  if (getObservation(*obs))
364  {
365  m_state = ssWorking;
366  appendObservation( obs );
367  }
368  else
369  {
370  m_state = ssError;
371  // THROW_EXCEPTION("No observation received from the USB board!");
372  }
373 }
374 
375 
376 /*-------------------------------------------------------------
377  initialize
378 -------------------------------------------------------------*/
379 /** Tries to open the camera, after setting all the parameters with a call to loadConfig.
380  * \exception This method must throw an exception with a descriptive message if some critical error is found.
381  */
382 void CBoardENoses::initialize()
383 {
384  // We'll rather try it in doProcess() since it's quite usual that it fails
385  // on a first try, then works on the next ones.
386  /*
387  if (!checkConnectionAndConnect())
388  THROW_EXCEPTION("Couldn't connect to the eNose board");
389  */
390 }
391 
392 /*-------------------------------------------------------------
393  setActiveChamber
394 -------------------------------------------------------------*/
395 /** Send to the MCE-nose the next Active Chamber */
396 
397 bool CBoardENoses::setActiveChamber( unsigned char chamber )
398 {
399  try
400  {
401  // Try to connect to the device:
402  CStream *comms = checkConnectionAndConnect();
403  if (!comms)
404  return false;
405 
406  // Send a byte to set the Active chamber on device.
407  // by default: Byte_to_send = 10_ _ _ _10
408  unsigned char buf[1];
409  buf[0] = ((chamber & 15) << 2) | 130; //130= 10 0000 10
410 
411  comms->WriteBuffer(buf,1); // Exceptions will be raised on errors here
412  return true;
413  }
414  catch(...)
415  {
416  // Close everything and start again:
417  delete_safe(m_stream_SERIAL);
418  delete_safe(m_stream_FTDI);
419  return false;
420  }
421 }
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
unsigned __int16 uint16_t
Definition: rptypes.h:46
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)
void WriteBuffer(const void *Buffer, size_t Count)
Writes a block of bytes to the stream from Buffer.
Definition: CStream.cpp:67
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.
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
#define M_PIf
math::TPose3D eNosePoseOnTheRobot
The pose of the sensors on the robot.
__int16 int16_t
Definition: rptypes.h:45
#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
void read_vector(const std::string &section, const std::string &name, const VECTOR_TYPE &defaultValue, VECTOR_TYPE &outValues, bool failIfNotFound=false) const
Reads a configuration parameter of type vector, stored in the file as a string: "[v1 v2 v3 ...
A class for interfacing an e-Noses via a FTDI USB link.
Definition: CBoardENoses.h:53
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.
void getContentAsString(std::string &str)
Gets the contents of the message as a string.
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
void sendMessage(const utils::CMessage &msg)
Send a message to the device.
Definition: CStream.cpp:556
std::vector< uint8_t > content
The contents of the message (memory is automatically handled by the std::vector object) ...
Definition: CMessage.h:33
mrpt::system::TTimeStamp BASE_IMPEXP secondsToTimestamp(const double nSeconds)
Transform a time interval (in seconds) into TTimeStamp (e.g.
Definition: datetime.cpp:219
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.
GLfloat GLfloat p
Definition: glext.h:5587
A class that contain generic messages, that can be sent and received from a "CClientTCPSocket" object...
Definition: CMessage.h:29
uint32_t type
An identifier of the message type (only the least-sig byte is typically sent)
Definition: CMessage.h:32



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