MRPT  1.9.9
CPhidgetInterfaceKitProximitySensors.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-2018, 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>
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  m_serialNumber(-1)
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  const mrpt::config::CConfigFileBase& configSource,
60  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 #else
185  MRPT_UNUSED_PARAM(configSource);
186  MRPT_UNUSED_PARAM(iniSection);
187 #endif
188 }
189 
190 /* -----------------------------------------------------
191  Initialize
192  ----------------------------------------------------- */
194 {
195 #if MRPT_HAS_PHIDGET
196  /*Try to connect to the interface kit board*/
197  CPhidgetInterfaceKit_create(
198  (CPhidgetInterfaceKitHandle*)m_carteInterfaceKit);
199  CPhidget_open(*((CPhidgetHandle*)(m_carteInterfaceKit)), m_serialNumber);
200  int err = CPhidget_waitForAttachment(
201  *((CPhidgetHandle*)(m_carteInterfaceKit)),
202  200); // wait 200ms for board attachment.
203  // if an error occur, "err" will be a > 0 value.
204  if (err > 0)
205  {
206  m_state = CGenericSensor::ssError;
208  "Can't find Phidget IK card, please check your serial number.");
209  }
210  // set frame rate
211  /*int miliseconds = static_cast<int>(1000./static_cast<float>(m_process_rate));
212  for(int i = 0 ; i < 8 ; i++)
213  {
214  if(m_sensorIsPlugged[i])
215  {
216  int err = CPhidgetInterfaceKit_setDataRate(*((CPhidgetInterfaceKitHandle*)(m_carteInterfaceKit)), i, miliseconds);
217  if(err > 0)
218  {
219  string error = format("Can't set process rate to %d ms on channel %d of the Phidget IK Board.", miliseconds, i);
220  m_state = CGenericSensor::ssError;
221  THROW_EXCEPTION(error);
222  }
223  }
224  }*/ // seems to be used only in the event based programming 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 = mrpt::make_aligned_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 -------------------------------------------------------------*/
276 {
277 #if MRPT_HAS_PHIDGET
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 =
309  2076. / (static_cast<float>(sensorValue) - 11.);
310  break;
311  case SHARP_80cm:
312  obsRange.sensedDistance =
313  4800. / (static_cast<float>(sensorValue) - 16.92);
314  break;
315  default:
316  obsRange.sensedDistance = -1;
317  break;
318  }
319 
320  obsRange.sensorID = i;
321  obsRange.sensorPose =
322  m_sensorPoses[i]
323  .asTPose(); // The pose of the IR sensor on the robot
324  obs.sensedData.push_back(obsRange);
325  }
326  }
327 #else
328  MRPT_UNUSED_PARAM(obs);
329 #endif
330 }
Declares a class derived from "CObservation" that encapsules a single range measurement, and associated parameters.
GLdouble GLdouble z
Definition: glext.h:3872
std::string read_string(const std::string &section, const std::string &name, const std::string &defaultValue, bool failIfNotFound=false) const
float minSensorDistance
The data members.
float sensorConeApperture
Cone aperture of each ultrasonic beam, in radians.
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:41
mrpt::system::TTimeStamp getCurrentTime()
Returns the current (UTC) system time.
Definition: datetime.h:82
double DEG2RAD(const double x)
Degrees to radians.
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.
float read_float(const std::string &section, const std::string &name, float defaultValue, bool failIfNotFound=false) const
STL namespace.
int read_int(const std::string &section, const std::string &name, int defaultValue, bool failIfNotFound=false) const
void loadConfig_sensorSpecific(const mrpt::config::CConfigFileBase &configSource, const std::string &iniSection)
See the class documentation at the top for expected parameters.
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).
Versatile class for consistent logging and management of output messages.
This namespace contains representation of robot actions and observations.
bool sectionExists(const std::string &section_name) const
Checks if a given section exists (name is case insensitive)
GLsizei const GLchar ** string
Definition: glext.h:4101
#define IMPLEMENTS_GENERIC_SENSOR(class_name, NameSpace)
This must be inserted in all CGenericSensor classes implementation files:
std::string sensorLabel
An arbitrary label that can be used to identify the sensor.
Definition: CObservation.h:62
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
mrpt::system::TTimeStamp timestamp
The associated UTC time-stamp.
Definition: CObservation.h:60
: 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:86
bool read_bool(const std::string &section, const std::string &name, bool defaultValue, bool failIfNotFound=false) const
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:16
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:32
math::TPose3D sensorPose
The 6D position of the sensor on the robot.
GLenum GLint GLint y
Definition: glext.h:3538
GLenum GLint x
Definition: glext.h:3538
void initialize()
Initialize the sensor according to the parameters previously read in the configuration file...
TMeasurementList sensedData
All the measurements.
#define MRPT_UNUSED_PARAM(a)
Determines whether this is an X86 or AMD64 platform.
Definition: common.h:186



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 7d5e6d718 Fri Aug 24 01:51:28 2018 +0200 at lun nov 2 08:35:50 CET 2020