MRPT  2.0.4
CPhidgetInterfaceKitProximitySensors.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 
14 #include <mrpt/system/os.h>
15 
16 #include <algorithm>
17 #include <iostream>
18 
19 #if MRPT_HAS_PHIDGET
20 #include <phidget21.h>
21 #endif
22 
23 using namespace mrpt::hwdrivers;
24 using namespace mrpt::obs;
25 using namespace mrpt::system;
26 using namespace std;
27 
29 
30 /* -----------------------------------------------------
31  Constructor
32  ----------------------------------------------------- */
35 
36 {
37 #if MRPT_HAS_PHIDGET
38  m_carteInterfaceKit = new CPhidgetInterfaceKitHandle;
39  *((CPhidgetInterfaceKitHandle*)m_carteInterfaceKit) = 0;
40  m_sensorLabel = "PhidgetInterfaceKit";
41 
42  m_sensorIsPlugged.assign(8, false);
43  m_minRange.assign(8, 0.1f);
44  m_maxRange.assign(8, 0.8f);
45  m_sensorPoses.resize(8);
46  m_sensorType.assign(8, UNPLUGGED);
47 
48 #else
50  "MRPT Was compiled without the CPhidget support. Recompile MRPT to use "
51  "this class");
52 #endif
53 }
54 
55 /* -----------------------------------------------------
56  loadConfig_sensorSpecific
57  ----------------------------------------------------- */
59  [[maybe_unused]] const mrpt::config::CConfigFileBase& configSource,
60  [[maybe_unused]] const std::string& iniSection)
61 {
62 #if MRPT_HAS_PHIDGET
63  if (!configSource.sectionExists(iniSection))
64  THROW_EXCEPTION("Can't find section in configuration file");
65  // looking for the board parameters.
66  // process_rate = 100 // Hz (common to all sensors)
67  // serialNumber = 12345 // The interface kit serial
68  // number.
69  m_process_rate =
70  configSource.read_int(iniSection, string("process_rate"), 50);
71  m_serialNumber =
72  configSource.read_int(iniSection, string("serialNumber"), -1);
73  bool display = configSource.read_bool(
74  iniSection, string("displayRecapitulativeInformations"), false);
75 
76  // Looking for each sensor.
77 
78  for (int i = 1; i <= 8; i++)
79  {
80  string sensorNKeyName = format("sensor%d", i);
81  string sensorType = configSource.read_string(
82  iniSection, sensorNKeyName, string("UNPLUGGED"));
83  if (sensorType != string("UNPLUGGED"))
84  {
85  // the sensor is plugged :
86  // // check if the sensor type is supported.
87  if (sensorType == string("EZ1"))
88  {
89  m_sensorType[i - 1] = EZ1;
90  m_minRange[i - 1] = 0.15; // meters
91  m_maxRange[i - 1] = 6.45; // meters
92  }
93  else if (sensorType == string("SHARP-30cm"))
94  {
95  m_sensorType[i - 1] = SHARP_30cm;
96  m_minRange[i - 1] = 0.04; // meters
97  m_maxRange[i - 1] = 0.3; // meters
98  }
99  else if (sensorType == string("SHARP-80cm"))
100  {
101  m_sensorType[i - 1] = SHARP_80cm;
102  m_minRange[i - 1] = 0.06; // meters
103  m_maxRange[i - 1] = 0.8; // meters
104  }
105  else
106  {
107  string err = format("Type of sensor %d is not supported", i);
108  m_state = CGenericSensor::ssError;
109  THROW_EXCEPTION(err);
110  }
111  m_sensorIsPlugged[i - 1] = true;
112  // reading the sensor pose.
113  string sensorNPoseX = format("pose%d_x", i);
114  string sensorNPoseY = format("pose%d_y", i);
115  string sensorNPoseZ = format("pose%d_z", i);
116  string sensorNPoseYaw = format("pose%d_yaw", i);
117  string sensorNPosePitch = format("pose%d_pitch", i);
118  string sensorNPoseRoll = format("pose%d_roll", i);
119 
120  float x = configSource.read_float(iniSection, sensorNPoseX, 0.0);
121  float y = configSource.read_float(iniSection, sensorNPoseY, 0.0);
122  float z = configSource.read_float(iniSection, sensorNPoseZ, 0.0);
123  float yaw =
124  configSource.read_float(iniSection, sensorNPoseYaw, 0.0);
125  float pitch =
126  configSource.read_float(iniSection, sensorNPosePitch, 0.0);
127  float roll =
128  configSource.read_float(iniSection, sensorNPoseRoll, 0.0);
129 
130  m_sensorPoses[i - 1] =
131  mrpt::poses::CPose3D(x, y, z, yaw, pitch, roll);
132  }
133  }
134  if (display)
135  { // width = 80;
136  cout.fill(' ');
137  cout << "+-------------------------------------------------------------"
138  "-----------------+"
139  << endl;
140  cout.width(79);
141  cout << "| Phidget interfaceKit board number : " << m_serialNumber;
142  cout << "|" << endl;
143  cout << "| Process rate : " << m_process_rate;
144  cout << "|" << endl;
145  cout << "+---------+---------------------+-----------------------------"
146  "-----------------+"
147  << endl;
148  cout << "| # + Sensor type | Sensor 3D pose "
149  " |"
150  << endl;
151  cout << "+---------+---------------------+-----------------------------"
152  "-----------------+"
153  << endl;
154  for (int i = 0; i < 8; i++)
155  {
156  cout << "|";
157  cout.width(9);
158  cout << i + 1;
159  cout << " |";
160  cout.width(19);
161  switch (m_sensorType[i])
162  {
163  case EZ1:
164  cout << "EZ1 |";
165  break;
166  case SHARP_30cm:
167  cout << "SHARP_30cm |";
168  break;
169  case SHARP_80cm:
170  cout << "SHARP_80cm |";
171  break;
172  case UNPLUGGED:
173  cout << "UNPLUGGED |";
174  break;
175  }
176  cout.width(43);
177  cout << m_sensorPoses[i];
178  cout << "|" << endl;
179  }
180  cout << "+-------------------------------------------------------------"
181  "-----------------+"
182  << endl;
183  }
184 #endif
185 }
186 
187 /* -----------------------------------------------------
188  Initialize
189  ----------------------------------------------------- */
191 {
192 #if MRPT_HAS_PHIDGET
193  /*Try to connect to the interface kit board*/
194  CPhidgetInterfaceKit_create(
195  (CPhidgetInterfaceKitHandle*)m_carteInterfaceKit);
196  CPhidget_open(*((CPhidgetHandle*)(m_carteInterfaceKit)), m_serialNumber);
197  int err = CPhidget_waitForAttachment(
198  *((CPhidgetHandle*)(m_carteInterfaceKit)),
199  200); // wait 200ms for board attachment.
200  // if an error occur, "err" will be a > 0 value.
201  if (err > 0)
202  {
203  m_state = CGenericSensor::ssError;
205  "Can't find Phidget IK card, please check your serial number.");
206  }
207  // set frame rate
208  /*int miliseconds =
209  static_cast<int>(1000./d2f(m_process_rate)); for(int i = 0 ;
210  i < 8 ; i++)
211  {
212  if(m_sensorIsPlugged[i])
213  {
214  int err =
215  CPhidgetInterfaceKit_setDataRate(*((CPhidgetInterfaceKitHandle*)(m_carteInterfaceKit)),
216  i, miliseconds); if(err > 0)
217  {
218  string error = format("Can't set process rate to %d ms on
219  channel %d of the Phidget IK Board.", miliseconds, i); m_state =
220  CGenericSensor::ssError; THROW_EXCEPTION(error);
221  }
222  }
223  }*/ // seems to be used only in the event based programming
224  // way.
225  // compute (min/max) of (min/max) ranges.
226  m_minOfMinRanges = *min_element(m_minRange.begin(), m_minRange.end());
227  m_maxOfMaxRanges = *max_element(m_maxRange.begin(), m_maxRange.end());
228  // driver is ready.
229  m_state = CGenericSensor::ssWorking;
230 #endif
231 }
232 
233 /* -----------------------------------------------------
234  Destructor
235  ----------------------------------------------------- */
237 {
238 #if MRPT_HAS_PHIDGET
239  if (*((CPhidgetHandle*)m_carteInterfaceKit))
240  {
241  CPhidget_close(*((CPhidgetHandle*)(m_carteInterfaceKit)));
242  CPhidget_delete(*((CPhidgetHandle*)(m_carteInterfaceKit)));
243  }
244 #endif
245 }
246 
247 /*-------------------------------------------------------------
248  doProcess
249 -------------------------------------------------------------*/
251 {
252  CObservationRange::Ptr obs = std::make_shared<CObservationRange>();
253 
254  try
255  {
256  getObservation(*obs);
257  m_state = ssWorking;
258  // if at least one data have been sensed :
259  if (obs->sensedData.size() > 0)
260  {
261  appendObservation(obs);
262  }
263  }
264  catch (...)
265  {
266  m_state = ssError;
267  THROW_EXCEPTION("No observation received from the Phidget board!");
268  }
269 }
270 
271 /*-------------------------------------------------------------
272  getObservation
273 -------------------------------------------------------------*/
275  [maybe_unused]] mrpt::obs::CObservationRange& obs)
276 {
277 #if MRPT_HAS_PHIDGET
278  obs.timestamp = mrpt::system::getCurrentTime();
279  obs.sensorLabel = m_sensorLabel;
280  obs.minSensorDistance = m_minOfMinRanges;
281  obs.maxSensorDistance = m_maxOfMaxRanges;
282  obs.sensorConeApperture =
283  DEG2RAD(2.0f); // TODO : Adapt to real sensor cone apperture.
284  obs.sensedData.clear();
285 
286  int sensorValue;
287  for (int i = 0; i < 8; i++)
288  {
289  if (m_sensorIsPlugged[i])
290  {
292  int err = CPhidgetInterfaceKit_getSensorValue(
293  *((CPhidgetInterfaceKitHandle*)(m_carteInterfaceKit)), i,
294  &sensorValue);
295  if (err > 0)
296  {
297  string error(
298  "Error durring acquiering sensor value on channel : %d", i);
300  }
301  switch (m_sensorType[i])
302  {
303  case EZ1:
304  // TODO : find the conversion formula.
305  obsRange.sensedDistance = 1.0;
306  break;
307  case SHARP_30cm:
308  obsRange.sensedDistance = 2076. / (d2f(sensorValue) - 11.);
309  break;
310  case SHARP_80cm:
311  obsRange.sensedDistance =
312  4800. / (d2f(sensorValue) - 16.92);
313  break;
314  default:
315  obsRange.sensedDistance = -1;
316  break;
317  }
318 
319  obsRange.sensorID = i;
320  obsRange.sensorPose =
321  m_sensorPoses[i]
322  .asTPose(); // The pose of the IR sensor on the robot
323  obs.sensedData.push_back(obsRange);
324  }
325  }
326 #endif
327 }
Declares a class derived from "CObservation" that encapsules a single range measurement, and associated parameters.
void loadConfig_sensorSpecific(const mrpt::config::CConfigFileBase &configSource, const std::string &iniSection) override
See the class documentation at the top for expected parameters.
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
std::string std::string format(std::string_view fmt, ARGS &&... args)
Definition: format.h:26
mrpt::system::TTimeStamp getCurrentTime()
Returns the current (UTC) system time.
Definition: datetime.h:82
void getObservation(mrpt::obs::CObservationRange &outObservation)
This method tries to get a set of range measurements from the IR sensors.
Contains classes for various device interfaces.
STL namespace.
float d2f(const double d)
shortcut for static_cast<float>(double)
This class allows loading and storing values and vectors of different types from a configuration text...
float sensedDistance
The measured range, in meters (or a value of 0 if there was no detected echo).
constexpr double DEG2RAD(const double x)
Degrees to radians.
Versatile class for consistent logging and management of output messages.
void initialize() override
Initialize the sensor according to the parameters previously read in the configuration file...
This namespace contains representation of robot actions and observations.
void doProcess() override
This method should be called periodically.
#define IMPLEMENTS_GENERIC_SENSOR(class_name, NameSpace)
This must be inserted in all CGenericSensor classes implementation files:
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
: An interface for the phidget Interface kit board (1018).
uint16_t sensorID
Some kind of sensor ID which identifies it on the bus (if applicable, 0 otherwise) ...
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
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
math::TPose3D sensorPose
The 6D position of the sensor on the robot.



Page generated by Doxygen 1.8.14 for MRPT 2.0.4 Git: 33de1d0ad Sat Jun 20 11:02:42 2020 +0200 at sáb jun 20 17:35:17 CEST 2020