MRPT  2.0.1
CBoardENoses.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "hwdrivers-precomp.h" // Precompiled headers
11 
13 #include <mrpt/math/ops_vectors.h>
15 #include <mrpt/system/os.h>
16 #include <iostream>
17 #include <memory>
18 #include <thread>
19 
20 using namespace mrpt::math;
21 using namespace mrpt::obs;
22 using namespace mrpt::poses;
23 using namespace mrpt::hwdrivers;
24 using namespace mrpt::io;
25 using namespace std;
26 
28 
29 /*-------------------------------------------------------------
30  CBoardENoses
31 -------------------------------------------------------------*/
32 CBoardENoses::CBoardENoses() : m_usbSerialNumber("ENOSE001"), m_COM_port()
33 {
34  m_sensorLabel = "ENOSE";
35  first_reading = true;
36 }
37 
38 /*-------------------------------------------------------------
39  loadConfig_sensorSpecific
40 -------------------------------------------------------------*/
41 void CBoardENoses::loadConfig_sensorSpecific(
42  const mrpt::config::CConfigFileBase& configSource,
43  const std::string& iniSection)
44 {
46 
47  m_usbSerialNumber =
48  configSource.read_string(iniSection, "USB_serialname", "", false);
49 
50 #ifdef _WIN32
51  m_COM_port =
52  configSource.read_string(iniSection, "COM_port_WIN", m_COM_port);
53 #else
54  m_COM_port =
55  configSource.read_string(iniSection, "COM_port_LIN", m_COM_port);
56 #endif
57  m_COM_baud =
58  configSource.read_uint64_t(iniSection, "COM_baudRate", m_COM_baud);
59 
60  configSource.read_vector(
61  iniSection, "enose_poses_x", vector<float>(0), enose_poses_x, true);
62  configSource.read_vector(
63  iniSection, "enose_poses_y", vector<float>(0), enose_poses_y, true);
64  configSource.read_vector(
65  iniSection, "enose_poses_z", vector<float>(0), enose_poses_z, true);
66 
67  configSource.read_vector(
68  iniSection, "enose_poses_yaw", vector<float>(0), enose_poses_yaw, true);
69  configSource.read_vector(
70  iniSection, "enose_poses_pitch", vector<float>(0), enose_poses_pitch,
71  true);
72  configSource.read_vector(
73  iniSection, "enose_poses_roll", vector<float>(0), enose_poses_roll,
74  true);
75 
76  ASSERT_(enose_poses_x.size() == enose_poses_y.size());
77  ASSERT_(enose_poses_x.size() == enose_poses_z.size());
78  ASSERT_(enose_poses_x.size() == enose_poses_yaw.size());
79  ASSERT_(enose_poses_x.size() == enose_poses_pitch.size());
80  ASSERT_(enose_poses_x.size() == enose_poses_roll.size());
81 
82  // Pass angles to radians:
83  enose_poses_yaw *= M_PIf / 180.0f;
84  enose_poses_pitch *= M_PIf / 180.0f;
85  enose_poses_roll *= M_PIf / 180.0f;
86 
87  MRPT_END
88 }
89 
90 /*-------------------------------------------------------------
91  queryFirmwareVersion
92 -------------------------------------------------------------*/
93 bool CBoardENoses::queryFirmwareVersion(string& out_firmwareVersion)
94 {
95  try
96  {
98 
99  // Try to connect to the device:
100  CStream* comms = checkConnectionAndConnect();
101  if (!comms) return false;
103 
104  msg.type = 0x10;
105  arch.sendMessage(msg);
106 
107  if (arch.receiveMessage(msgRx))
108  {
109  msgRx.getContentAsString(out_firmwareVersion);
110  return true;
111  }
112  else
113  return false;
114  }
115  catch (...)
116  {
117  // Close everything and start again:
118  m_stream_SERIAL.reset();
119  m_stream_FTDI.reset();
120  return false;
121  }
122 }
123 
124 /*-------------------------------------------------------------
125  checkConnectionAndConnect
126 -------------------------------------------------------------*/
127 CStream* CBoardENoses::checkConnectionAndConnect()
128 {
129  // Make sure one of the two possible pipes is open:
130  if (!m_stream_FTDI && !m_stream_SERIAL)
131  {
132  if (!m_COM_port.empty())
133  m_stream_SERIAL = std::make_unique<mrpt::comms::CSerialPort>();
134  else
135  m_stream_FTDI = std::make_unique<mrpt::comms::CInterfaceFTDI>();
136  }
137 
138  if (m_stream_FTDI)
139  { // FTDI pipe ==================
140  if (m_stream_FTDI->isOpen()) return m_stream_FTDI.get();
141  try
142  {
143  m_stream_FTDI->OpenBySerialNumber(m_usbSerialNumber);
144  std::this_thread::sleep_for(10ms);
145  m_stream_FTDI->Purge();
146  std::this_thread::sleep_for(10ms);
147  m_stream_FTDI->SetLatencyTimer(1);
148  m_stream_FTDI->SetTimeouts(10, 100);
149  return m_stream_FTDI.get();
150  }
151  catch (...)
152  { // Error opening device:
153  m_stream_FTDI->Close();
154  return nullptr;
155  }
156  }
157  else
158  { // Serial pipe ==================
159  ASSERT_(m_stream_SERIAL);
160  if (m_stream_SERIAL->isOpen()) return m_stream_SERIAL.get();
161  try
162  {
163  m_stream_SERIAL->open(m_COM_port);
164  m_stream_SERIAL->setConfig(m_COM_baud);
165  std::this_thread::sleep_for(10ms);
166  m_stream_SERIAL->purgeBuffers();
167  std::this_thread::sleep_for(10ms);
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.get();
171  }
172  catch (...)
173  { // Error opening device:
174  m_stream_SERIAL->close();
175  return nullptr;
176  }
177  }
178 }
179 
180 /*-------------------------------------------------------------
181  getObservation
182 -------------------------------------------------------------*/
184 {
185  try
186  {
187  // Connected?
188  CStream* comms = checkConnectionAndConnect();
189 
190  if (!comms) return false;
191 
194 
195  obs.m_readings.clear();
196 
197  //// Send request:
198  // msg.type = 0x11;
199  // msg.content.clear();
200  // comms->sendMessage( msg );
201 
202  //----------------------------MCE-nose
203  // FRAME--------------------------------------------------
204  // Wait for e-nose frame: <0x69><0x91><lenght><body><0x96> "Bytes"
205  // Where <body> = [Numchamber, Activechamber, N sensors*M chambers*2, 2
206  // timestamp] of uint16_t
207  // MCE-nose provides a 136B body lenght which makes 140B total frame
208  // lenght
209 
211  if (!arch.receiveMessage(msg))
212  {
213  return false;
214  }
215 
216  // m_state = ssWorking;
217 
218  // Copy to "uint16_t":
219  ASSERT_((msg.content.size() % 2) == 0);
220 
221  vector<uint16_t> readings(
222  msg.content.size() /
223  2); // divide by 2 to pass from byte to word. 136B/2 = 68 Words
224 
225  if (msg.content.size() > 0)
226  {
227  // Copy to a vector of 16bit integers:
228  memcpy(
229  &readings[0], &msg.content[0],
230  msg.content.size() * sizeof(msg.content[0]));
231 
232  // HEADER Frame [ NÂș of chambers/enoses (16b) , Active Chamber
233  // (16b)]
234  auto NumberOfChambers = (size_t)readings[0];
235  auto ActiveChamber = (size_t)readings[1];
236 
237  // Sensors readings info
238  ASSERT_(((readings.size() - 4) % NumberOfChambers) == 0);
239  size_t wordsPereNose = (readings.size() - 4) / NumberOfChambers;
240 
241  // Process each chamber
242  for (size_t i = 0; i < NumberOfChambers; i++)
243  {
244  // ----------------------------------------------------------------------
245  // Each "i" comprises a complete Enose reading: Gas sensors +
246  // temperature
247  // ----------------------------------------------------------------------
248 
249  // Do we have the sensor position?
250  if (i < enose_poses_x.size())
251  {
252  newRead.eNosePoseOnTheRobot = TPose3D(
253  enose_poses_x[i], enose_poses_y[i], enose_poses_z[i],
254  enose_poses_yaw[i], enose_poses_pitch[i],
255  enose_poses_roll[i]);
256  }
257  else
258  newRead.eNosePoseOnTheRobot = TPose3D(0, 0, 0, 0, 0, 0);
259 
260  // Process the sensor codes:
261  newRead.sensorTypes.clear();
262  newRead.readingsVoltage.clear();
263  newRead.hasTemperature = false;
264  newRead.isActive = false;
265 
266  // check if active chamber
267  if (i == (ActiveChamber)) newRead.isActive = true;
268 
269  // process each sensor on this chamber "i"
270  for (size_t idx = 0; idx < wordsPereNose / 2; idx++)
271  {
272  if (readings[i * wordsPereNose + 2 * idx + 2] !=
273  0x0000) // not empty slot
274  {
275  // Is temperature?
276  if (readings[i * wordsPereNose + 2 * idx + 2] == 0xFFFF)
277  {
278  newRead.hasTemperature = true;
279  newRead.temperature =
280  ((int16_t)readings
281  [i * wordsPereNose + 2 * idx + 3]) /
282  32.0f;
283  }
284  else // Is a gas sensors
285  {
286  // It is not a null code: There is a valid measure:
287  newRead.sensorTypes.push_back(
288  readings[i * wordsPereNose + 2 * idx + 2]);
289 
290  // Pass from ADC value[12bits] to [0-2.5] volt
291  // range:
292  newRead.readingsVoltage.push_back(
293  (readings[i * wordsPereNose + 2 * idx + 3] *
294  5.0f) /
295  4096.0f);
296  }
297  }
298  } // end for each sensor on this eNose
299 
300  // Add to observations:
301  if (!newRead.sensorTypes.empty())
302  obs.m_readings.push_back(newRead);
303 
304  } // end for each i'th eNose
305 
306  obs.sensorLabel = m_sensorLabel;
307 
308  // Set Timestamp
309  auto* p =
310  (uint16_t*)&readings[readings.size() - 2]; // Get readings time
311  // from frame
312  // (always last 2
313  // words)
314  obs.timestamp =
315  mrpt::system::time_tToTimestamp(((double)*p) / 1000);
316 
317  if (first_reading)
318  {
319  initial_timestamp =
321  first_reading = false;
322  }
323  obs.timestamp = obs.timestamp + initial_timestamp;
324 
325  } // end if message has data
326 
327  // CONTROL
328  bool correct = true;
329 
330  if (obs.m_readings.size() != 4)
331  correct = false;
332  else
333  {
334  for (auto& m_reading : obs.m_readings)
335  {
336  if ((m_reading.sensorTypes.size() != 7) ||
337  (m_reading.readingsVoltage.size() != 7))
338  correct = false;
339  else
340  {
341  }
342  }
343  }
344 
345  if (!correct) printf("Error en la observacion"); // For debug
346 
347  return !obs.m_readings.empty(); // Done OK!
348  }
349  catch (exception& e)
350  {
351  cerr << "[CBoardENoses::getObservation] Returning false due to "
352  "exception: "
353  << endl;
354  cerr << e.what() << endl;
355  return false;
356  }
357  catch (...)
358  {
359  return false;
360  }
361 }
362 
363 /*-------------------------------------------------------------
364  doProcess
365 -------------------------------------------------------------*/
366 /** This method should be called periodically (at least at 1Hz to capture ALL
367  * the real-time data)
368  * It is thread safe, i.e. you can call this from one thread, then to other
369  * methods from other threads.
370  */
371 void CBoardENoses::doProcess()
372 {
374  std::make_shared<CObservationGasSensors>();
375 
376  if (getObservation(*obs))
377  {
378  m_state = ssWorking;
379  appendObservation(obs);
380  }
381  else
382  {
383  m_state = ssError;
384  // THROW_EXCEPTION("No observation received from the USB board!");
385  }
386 }
387 
388 /*-------------------------------------------------------------
389  initialize
390 -------------------------------------------------------------*/
391 /** Tries to open the camera, after setting all the parameters with a call to
392  * loadConfig.
393  * \exception This method must throw an exception with a descriptive message
394  * if some critical error is found.
395  */
397 {
398  // We'll rather try it in doProcess() since it's quite usual that it fails
399  // on a first try, then works on the next ones.
400  /*
401  if (!checkConnectionAndConnect())
402  THROW_EXCEPTION("Couldn't connect to the eNose board");
403  */
404 }
405 
406 /*-------------------------------------------------------------
407  setActiveChamber
408 -------------------------------------------------------------*/
409 /** Send to the MCE-nose the next Active Chamber */
410 
411 bool CBoardENoses::setActiveChamber(unsigned char chamber)
412 {
413  try
414  {
415  // Try to connect to the device:
416  CStream* comms = checkConnectionAndConnect();
417  if (!comms) return false;
418 
419  // Send a byte to set the Active chamber on device.
420  // by default: Byte_to_send = 10_ _ _ _10
421  unsigned char buf[1];
422  buf[0] = ((chamber & 15) << 2) | 130; // 130= 10 0000 10
423 
424  comms->Write(buf, 1); // Exceptions will be raised on errors here
425  return true;
426  }
427  catch (...)
428  {
429  // Close everything and start again:
430  m_stream_SERIAL.reset();
431  m_stream_FTDI.reset();
432  return false;
433  }
434 }
#define MRPT_START
Definition: exceptions.h:241
std::string read_string(const std::string &section, const std::string &name, const std::string &defaultValue, bool failIfNotFound=false) const
app initialize(argc, argv)
mrpt::system::TTimeStamp getCurrentTime()
Returns the current (UTC) system time.
Definition: datetime.h:82
float temperature
Sensed temperature in Celcius (valid if hasTemperature=true only)
uint32_t type
An identifier of the message type (only the least-sig byte is typically sent)
Definition: CMessage.h:32
Contains classes for various device interfaces.
STL namespace.
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: io/CStream.h:28
CArchiveStreamBase< STREAM > archiveFrom(STREAM &s)
Helper function to create a templatized wrapper CArchive object for a: MRPT&#39;s CStream, std::istream, std::ostream, std::stringstream.
Definition: CArchive.h:592
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
This class allows loading and storing values and vectors of different types from a configuration text...
This base provides a set of functions for maths stuff.
math::TPose3D eNosePoseOnTheRobot
The pose of the sensors on the robot.
std::vector< int > sensorTypes
The kind of sensors in the array (size of "sensorTypes" is the same that the size of "readingsVoltage...
A class for interfacing an e-Noses via a FTDI USB link.
Definition: CBoardENoses.h:54
bool hasTemperature
Must be true for "temperature" to contain a valid measurement.
This namespace contains representation of robot actions and observations.
#define M_PIf
Definition: common.h:61
uint64_t read_uint64_t(const std::string &section, const std::string &name, uint64_t defaultValue, bool failIfNotFound=false) const
Declares a class derived from "CObservation" that represents a set of readings from gas sensors...
#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...
std::string sensorLabel
An arbitrary label that can be used to identify the sensor.
Definition: CObservation.h:62
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.
Definition: CObservation.h:60
A class that contain generic messages, that can be sent and received from a "CClientTCPSocket" object...
Definition: CMessage.h:27
void getContentAsString(std::string &str)
Gets the contents of the message as a string.
Definition: CMessage.cpp:96
std::vector< TObservationENose > m_readings
One entry per e-nose on the robot.
OBSERVATION_T::Ptr getObservation(mrpt::obs::CSensoryFrame::Ptr &observations, mrpt::obs::CObservation::Ptr &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:31
#define MRPT_END
Definition: exceptions.h:245
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
Definition: TPose3D.h:24
std::vector< uint8_t > content
The contents of the message (memory is automatically handled by the std::vector object) ...
Definition: CMessage.h:35
std::vector< float > readingsVoltage
The set of readings (in volts) from the array of sensors (size of "sensorTypes" is the same that the ...
mrpt::system::TTimeStamp time_tToTimestamp(const double t)
Transform from standard "time_t" (actually a double number, it can contain fractions of seconds) to T...
Definition: datetime.h:91
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 ...
void memcpy(void *dest, size_t destSize, const void *src, size_t copyCount) noexcept
An OS and compiler independent version of "memcpy".



Page generated by Doxygen 1.8.14 for MRPT 2.0.1 Git: 0fef1a6d7 Fri Apr 3 23:00:21 2020 +0200 at vie abr 3 23:20:28 CEST 2020