MRPT  1.9.9
CEnoseModular.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 
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  CEnoseModular
31 -------------------------------------------------------------*/
32 CEnoseModular::CEnoseModular() : m_usbSerialNumber("ENOSE002"), m_COM_port()
33 {
34  m_sensorLabel = "EnoseModular";
35  first_reading = true;
36 }
37 
38 /*-------------------------------------------------------------
39  loadConfig_sensorSpecific
40 -------------------------------------------------------------*/
41 void CEnoseModular::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 = configSource.read_string(iniSection, "COM_port_WIN", "COM1");
52 #else
53  m_COM_port =
54  configSource.read_string(iniSection, "COM_port_LIN", m_COM_port);
55 #endif
56  m_COM_baud =
57  configSource.read_uint64_t(iniSection, "COM_baudRate", m_COM_baud);
58 
59  MRPT_END
60 }
61 
62 /*-------------------------------------------------------------
63  checkConnectionAndConnect
64 -------------------------------------------------------------*/
65 CStream* CEnoseModular::checkConnectionAndConnect()
66 {
67  // Make sure one of the two possible pipes is open:
68  if (!m_stream_FTDI && !m_stream_SERIAL)
69  {
70  if (!m_COM_port.empty())
71  m_stream_SERIAL = std::make_unique<mrpt::comms::CSerialPort>();
72  else
73  m_stream_FTDI = std::make_unique<mrpt::comms::CInterfaceFTDI>();
74  }
75 
76  if (m_stream_FTDI)
77  { // FTDI pipe ==================
78  if (m_stream_FTDI->isOpen()) return m_stream_FTDI.get();
79  try
80  {
81  m_stream_FTDI->OpenBySerialNumber(m_usbSerialNumber);
82  std::this_thread::sleep_for(10ms);
83  m_stream_FTDI->Purge();
84  std::this_thread::sleep_for(10ms);
85  m_stream_FTDI->SetLatencyTimer(1);
86  m_stream_FTDI->SetTimeouts(10, 100);
87  return m_stream_FTDI.get();
88  }
89  catch (...)
90  { // Error opening device:
91  m_stream_FTDI->Close();
92  return nullptr;
93  }
94  }
95  else
96  { // Serial pipe ==================
97  ASSERT_(m_stream_SERIAL);
98  if (m_stream_SERIAL->isOpen()) return m_stream_SERIAL.get();
99  try
100  {
101  m_stream_SERIAL->open(m_COM_port);
102  m_stream_SERIAL->setConfig(m_COM_baud);
103  // m_stream_SERIAL->setTimeouts(25,1,100, 1,20);
104  m_stream_SERIAL->setTimeouts(50, 1, 100, 1, 20);
105  std::this_thread::sleep_for(10ms);
106  m_stream_SERIAL->purgeBuffers();
107  std::this_thread::sleep_for(10ms);
108  return m_stream_SERIAL.get();
109  }
110  catch (...)
111  { // Error opening device:
112  m_stream_SERIAL->close();
113  return nullptr;
114  }
115  }
116 }
117 
118 /*-------------------------------------------------------------
119  getObservation
120 -------------------------------------------------------------*/
122 {
123  try
124  {
125  // Connected?
126  CStream* comms = checkConnectionAndConnect();
127 
128  if (!comms)
129  {
130  cout << "ERORR: Problem connecting to Device." << endl;
131  return false;
132  }
133 
135  obs.m_readings.clear();
136 
137  //---------------------------- Enose Modular FRAME
138  //--------------------------------------------------
139  // Wait for e-nose frame: <0x69><0x91><lenght><body><0x96> "Bytes"
140  // Where <body> = <temp>[<SensorID_H><SensorID_L><Sensor_Value>] x
141  // N_senosrs
142  // Modular-nose provides a 4B+body frame lenght
143 
145  bool time_out = false;
147  double time_out_val = 1; // seconds
148 
150 
151  while (!arch.receiveMessage(msg) && !time_out)
152  {
154  time_out_val)
155  time_out = true;
156  }
157 
158  if (time_out)
159  {
160  cout << "[CEnoseModular - getObservation] measurement Timed-Out"
161  << endl;
162  return false;
163  }
164 
165  if (msg.content.size() > 0)
166  {
167  // Each sensor reading is composed of 3 Bytes
168  // [<SensorID_H><SensorID_L><Sensor_Value>]
169  ASSERT_((msg.content.size() - 1) % 3 == 0);
170  size_t numSensors = (msg.content.size() - 1) / 3;
171 
172  // Prepare the Enose observation
173  newRead.sensorTypes.clear();
174  newRead.readingsVoltage.clear();
175  newRead.hasTemperature = true;
176  newRead.isActive = true;
177 
178  // Do we have the sensor position?
179  if (enose_poses_x.size() != 0)
180  {
181  newRead.eNosePoseOnTheRobot = TPose3D(
182  enose_poses_x[0], enose_poses_y[0], enose_poses_z[0],
183  enose_poses_yaw[0], enose_poses_pitch[0],
184  enose_poses_roll[0]);
185  }
186  else
187  newRead.eNosePoseOnTheRobot = TPose3D(0, 0, 0, 0, 0, 0);
188 
189  // Get Temperature (degrees C)
190  newRead.temperature = msg.content[0] * 1.65214 - 277.74648;
191 
192  // process all sensors
193  for (size_t idx = 0; idx < numSensors; idx++)
194  {
195  // Copy ID (2 BYTES) To integer
196  int sensorType_temp = 0;
197  // memcpy( &sensorType, &msg.content[idx*3+1],
198  // 2*sizeof(msg.content[0]) );
199  memcpy(
200  &sensorType_temp, &msg.content[idx * 3 + 1],
201  sizeof(msg.content[0]));
202  int sensorType = sensorType_temp << (8);
203  memcpy(
204  &sensorType, &msg.content[idx * 3 + 2],
205  sizeof(msg.content[0]));
206 
207  // Add sensor Type (ID)
208  newRead.sensorTypes.push_back(sensorType);
209 
210  // Transform from ADC value[8bits] to [0-0.6] volt range:
211  newRead.readingsVoltage.push_back(
212  (msg.content[idx * 3 + 3] * 0.6f) / 255.0f);
213  }
214 
215  // Purge buffers
216  purgeBuffers();
217 
218  // Add data to observation:
219  obs.m_readings.push_back(newRead);
220  obs.sensorLabel = m_sensorLabel;
222  return !obs.m_readings.empty(); // Done OK!
223  }
224  else
225  {
226  cout << "Message was empty" << endl;
227  return false;
228  }
229  }
230  catch (exception& e)
231  {
232  cerr << "[CEnoseModular::getObservation] Returning false due to "
233  "exception: "
234  << endl;
235  cerr << e.what() << endl;
236  return false;
237  }
238  catch (...)
239  {
240  return false;
241  }
242 }
243 
244 /*-------------------------------------------------------------
245  doProcess
246 -------------------------------------------------------------*/
247 /** This method should be called periodically (at least at 1Hz to capture ALL
248  * the real-time data)
249  * It is thread safe, i.e. you can call this from one thread, then to other
250  * methods from other threads.
251  */
252 void CEnoseModular::doProcess()
253 {
255  std::make_shared<CObservationGasSensors>();
256 
257  if (getObservation(*obs))
258  {
259  m_state = ssWorking;
260  appendObservation(obs);
261  }
262  else
263  {
264  m_state = ssError;
265  cout << "No observation received from the USB board!" << endl;
266  // THROW_EXCEPTION("No observation received from the USB board!");
267  }
268 }
269 
270 /*-------------------------------------------------------------
271  purgeBuffers
272 -------------------------------------------------------------*/
273 void CEnoseModular::purgeBuffers()
274 {
275  if (!checkConnectionAndConnect()) return;
276 
277  if (m_stream_FTDI)
278  { // FTDI pipe
279  m_stream_FTDI->Purge();
280  }
281  else
282  { // Serial pipe
283  m_stream_SERIAL->purgeBuffers();
284  }
285 }
#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
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)
mrpt::system::TTimeStamp now()
A shortcut for system::getCurrentTime.
Definition: datetime.h:86
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
mrpt::Clock::time_point TTimeStamp
A system independent time type, it holds the the number of 100-nanosecond intervals since January 1...
Definition: datetime.h:40
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...
bool hasTemperature
Must be true for "temperature" to contain a valid measurement.
This namespace contains representation of robot actions and observations.
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
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
double 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.h:123
std::vector< float > readingsVoltage
The set of readings (in volts) from the array of sensors (size of "sensorTypes" is the same that the ...
A class for interfacing an e-NoseModular via a FTDI USB link.
Definition: CEnoseModular.h:53
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 1.9.9 Git: c7a3bec24 Sun Mar 29 18:33:13 2020 +0200 at dom mar 29 18:50:38 CEST 2020