MRPT  1.9.9
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-2019, 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  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 =
212  static_cast<int>(1000./static_cast<float>(m_process_rate)); for(int i = 0 ;
213  i < 8 ; i++)
214  {
215  if(m_sensorIsPlugged[i])
216  {
217  int err =
218  CPhidgetInterfaceKit_setDataRate(*((CPhidgetInterfaceKitHandle*)(m_carteInterfaceKit)),
219  i, miliseconds); if(err > 0)
220  {
221  string error = format("Can't set process rate to %d ms on
222  channel %d of the Phidget IK Board.", miliseconds, i); m_state =
223  CGenericSensor::ssError; THROW_EXCEPTION(error);
224  }
225  }
226  }*/ // seems to be used only in the event based programming
227  // way.
228  // compute (min/max) of (min/max) ranges.
229  m_minOfMinRanges = *min_element(m_minRange.begin(), m_minRange.end());
230  m_maxOfMaxRanges = *max_element(m_maxRange.begin(), m_maxRange.end());
231  // driver is ready.
232  m_state = CGenericSensor::ssWorking;
233 #endif
234 }
235 
236 /* -----------------------------------------------------
237  Destructor
238  ----------------------------------------------------- */
240 {
241 #if MRPT_HAS_PHIDGET
242  if (*((CPhidgetHandle*)m_carteInterfaceKit))
243  {
244  CPhidget_close(*((CPhidgetHandle*)(m_carteInterfaceKit)));
245  CPhidget_delete(*((CPhidgetHandle*)(m_carteInterfaceKit)));
246  }
247 #endif
248 }
249 
250 /*-------------------------------------------------------------
251  doProcess
252 -------------------------------------------------------------*/
254 {
255  CObservationRange::Ptr obs = std::make_shared<CObservationRange>();
256 
257  try
258  {
259  getObservation(*obs);
260  m_state = ssWorking;
261  // if at least one data have been sensed :
262  if (obs->sensedData.size() > 0)
263  {
264  appendObservation(obs);
265  }
266  }
267  catch (...)
268  {
269  m_state = ssError;
270  THROW_EXCEPTION("No observation received from the Phidget board!");
271  }
272 }
273 
274 /*-------------------------------------------------------------
275  getObservation
276 -------------------------------------------------------------*/
279 {
280 #if MRPT_HAS_PHIDGET
282  obs.sensorLabel = m_sensorLabel;
283  obs.minSensorDistance = m_minOfMinRanges;
284  obs.maxSensorDistance = m_maxOfMaxRanges;
285  obs.sensorConeApperture =
286  DEG2RAD(2.0f); // TODO : Adapt to real sensor cone apperture.
287  obs.sensedData.clear();
288 
289  int sensorValue;
290  for (int i = 0; i < 8; i++)
291  {
292  if (m_sensorIsPlugged[i])
293  {
295  int err = CPhidgetInterfaceKit_getSensorValue(
296  *((CPhidgetInterfaceKitHandle*)(m_carteInterfaceKit)), i,
297  &sensorValue);
298  if (err > 0)
299  {
300  string error(
301  "Error durring acquiering sensor value on channel : %d", i);
303  }
304  switch (m_sensorType[i])
305  {
306  case EZ1:
307  // TODO : find the conversion formula.
308  obsRange.sensedDistance = 1.0;
309  break;
310  case SHARP_30cm:
311  obsRange.sensedDistance =
312  2076. / (static_cast<float>(sensorValue) - 11.);
313  break;
314  case SHARP_80cm:
315  obsRange.sensedDistance =
316  4800. / (static_cast<float>(sensorValue) - 16.92);
317  break;
318  default:
319  obsRange.sensedDistance = -1;
320  break;
321  }
322 
323  obsRange.sensorID = i;
324  obsRange.sensorPose =
325  m_sensorPoses[i]
326  .asTPose(); // The pose of the IR sensor on the robot
327  obs.sensedData.push_back(obsRange);
328  }
329  }
330 #else
331  MRPT_UNUSED_PARAM(obs);
332 #endif
333 }
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.
GLdouble GLdouble z
Definition: glext.h:3879
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:67
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
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.
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.
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:4116
#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:84
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:31
math::TPose3D sensorPose
The 6D position of the sensor on the robot.
GLenum GLint GLint y
Definition: glext.h:3542
GLenum GLint x
Definition: glext.h:3542
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: 1de0e027c Sat Sep 14 16:15:22 2019 +0200 at sáb sep 14 16:20:14 CEST 2019