Main MRPT website > C++ reference for MRPT 1.5.6
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-2017, 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 
18 #if MRPT_HAS_PHIDGET
19  #include <phidget21.h>
20 #endif
21 
22 using namespace mrpt::hwdrivers;
23 using namespace mrpt::obs;
24 using namespace mrpt::system;
25 using namespace mrpt::utils;
26 using namespace std;
27 
29 
30 /* -----------------------------------------------------
31  Constructor
32  ----------------------------------------------------- */
34  mrpt::utils::COutputLogger("CPhidgetInterfaceKitProximitySensors"),
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
49  THROW_EXCEPTION("MRPT Was compiled without the CPhidget support. Recompile MRPT to use this class")
50 #endif
51 }
52 
53 /* -----------------------------------------------------
54  loadConfig_sensorSpecific
55  ----------------------------------------------------- */
57  const mrpt::utils::CConfigFileBase &configSource,
58  const std::string &iniSection )
59 {
60 #if MRPT_HAS_PHIDGET
61  if(!configSource.sectionExists(iniSection)) THROW_EXCEPTION("Can't find section in configuration file");
62  // looking for the board parameters.
63  // process_rate = 100 // Hz (common to all sensors)
64  // serialNumber = 12345 // The interface kit serial number.
65  m_process_rate = configSource.read_int(iniSection, string("process_rate"), 50);
66  m_serialNumber = configSource.read_int(iniSection, string("serialNumber"), -1);
67  bool display = configSource.read_bool(iniSection, string("displayRecapitulativeInformations"), false);
68 
69  // Looking for each sensor.
70 
71  for(int i = 1 ; i <= 8 ; i++)
72  {
73  string sensorNKeyName = format("sensor%d", i);
74  string sensorType = configSource.read_string(iniSection, sensorNKeyName, string("UNPLUGGED"));
75  if(sensorType != string("UNPLUGGED"))
76  {
77  // the sensor is plugged :
78  // // check if the sensor type is supported.
79  if(sensorType == string("EZ1"))
80  {
81  m_sensorType[i-1] = EZ1;
82  m_minRange[i-1] = 0.15; // meters
83  m_maxRange[i-1] = 6.45; // meters
84  }else if(sensorType == string("SHARP-30cm"))
85  {
86  m_sensorType[i-1] = SHARP_30cm;
87  m_minRange[i-1] = 0.04; // meters
88  m_maxRange[i-1] = 0.3; // meters
89  }else if(sensorType == string("SHARP-80cm"))
90  {
91  m_sensorType[i-1] = SHARP_80cm;
92  m_minRange[i-1] = 0.06; // meters
93  m_maxRange[i-1] = 0.8; // meters
94  }else
95  {
96  string err = format("Type of sensor %d is not supported", i);
97  m_state = CGenericSensor::ssError;
98  THROW_EXCEPTION(err);
99  }
100  m_sensorIsPlugged[i-1] = true;
101  // reading the sensor pose.
102  string sensorNPoseX = format("pose%d_x", i);
103  string sensorNPoseY = format("pose%d_y", i);
104  string sensorNPoseZ = format("pose%d_z", i);
105  string sensorNPoseYaw = format("pose%d_yaw", i);
106  string sensorNPosePitch = format("pose%d_pitch", i);
107  string sensorNPoseRoll = format("pose%d_roll", i);
108 
109  float x = configSource.read_float(iniSection, sensorNPoseX, 0.0);
110  float y = configSource.read_float(iniSection, sensorNPoseY, 0.0);
111  float z = configSource.read_float(iniSection, sensorNPoseZ, 0.0);
112  float yaw = configSource.read_float(iniSection, sensorNPoseYaw, 0.0);
113  float pitch = configSource.read_float(iniSection, sensorNPosePitch, 0.0);
114  float roll = configSource.read_float(iniSection, sensorNPoseRoll, 0.0);
115 
116  m_sensorPoses[i-1] = mrpt::poses::CPose3D(x,y,z,yaw,pitch,roll);
117  }
118  }
119  if(display)
120  { // width = 80;
121  cout.fill(' ');
122  cout << "+------------------------------------------------------------------------------+" << endl;
123  cout.width(79);
124  cout << "| Phidget interfaceKit board number : " << m_serialNumber;
125  cout << "|" << endl;
126  cout << "| Process rate : " << m_process_rate;
127  cout << "|" << endl;
128  cout << "+---------+---------------------+----------------------------------------------+" << endl;
129  cout << "| # + Sensor type | Sensor 3D pose |" << endl;
130  cout << "+---------+---------------------+----------------------------------------------+" << endl;
131  for(int i = 0 ; i < 8 ; i++)
132  {
133  cout << "|";
134  cout.width(9);
135  cout << i+1;
136  cout << " |";
137  cout.width(19);
138  switch (m_sensorType[i])
139  {
140  case EZ1 :
141  cout << "EZ1 |";
142  break;
143  case SHARP_30cm :
144  cout << "SHARP_30cm |";
145  break;
146  case SHARP_80cm :
147  cout << "SHARP_80cm |";
148  break;
149  case UNPLUGGED :
150  cout << "UNPLUGGED |";
151  break;
152  }
153  cout.width(43);
154  cout << m_sensorPoses[i];
155  cout << "|" << endl;
156  }
157  cout << "+------------------------------------------------------------------------------+" << endl;
158  }
159 #else
160  MRPT_UNUSED_PARAM(configSource);
161  MRPT_UNUSED_PARAM(iniSection);
162 #endif
163 }
164 
165 /* -----------------------------------------------------
166  Initialize
167  ----------------------------------------------------- */
169 {
170 #if MRPT_HAS_PHIDGET
171  /*Try to connect to the interface kit board*/
172  CPhidgetInterfaceKit_create((CPhidgetInterfaceKitHandle*)m_carteInterfaceKit);
173  CPhidget_open(*((CPhidgetHandle*)(m_carteInterfaceKit)), m_serialNumber);
174  int err = CPhidget_waitForAttachment(*((CPhidgetHandle*)(m_carteInterfaceKit)), 200); // wait 200ms for board attachment.
175  // if an error occur, "err" will be a > 0 value.
176  if(err > 0 )
177  {
178  m_state = CGenericSensor::ssError;
179  THROW_EXCEPTION("Can't find Phidget IK card, please check your serial number.");
180  }
181  // set frame rate
182  /*int miliseconds = static_cast<int>(1000./static_cast<float>(m_process_rate));
183  for(int i = 0 ; i < 8 ; i++)
184  {
185  if(m_sensorIsPlugged[i])
186  {
187  int err = CPhidgetInterfaceKit_setDataRate(*((CPhidgetInterfaceKitHandle*)(m_carteInterfaceKit)), i, miliseconds);
188  if(err > 0)
189  {
190  string error = format("Can't set process rate to %d ms on channel %d of the Phidget IK Board.", miliseconds, i);
191  m_state = CGenericSensor::ssError;
192  THROW_EXCEPTION(error);
193  }
194  }
195  }*/ // seems to be used only in the event based programming way.
196  // compute (min/max) of (min/max) ranges.
197  m_minOfMinRanges = *min_element(m_minRange.begin(), m_minRange.end());
198  m_maxOfMaxRanges = *max_element(m_maxRange.begin(), m_maxRange.end());
199  // driver is ready.
200  m_state = CGenericSensor::ssWorking;
201 #endif
202 }
203 
204 
205 /* -----------------------------------------------------
206  Destructor
207  ----------------------------------------------------- */
209 {
210 #if MRPT_HAS_PHIDGET
211  if(*((CPhidgetHandle*)m_carteInterfaceKit))
212  {
213  CPhidget_close(*((CPhidgetHandle*)(m_carteInterfaceKit)));
214  CPhidget_delete(*((CPhidgetHandle*)(m_carteInterfaceKit)));
215  }
216 #endif
217 }
218 
219 /*-------------------------------------------------------------
220  doProcess
221 -------------------------------------------------------------*/
223 {
224  CObservationRangePtr obs= CObservationRange::Create();
225 
226  try
227  {
228  getObservation(*obs);
229  m_state = ssWorking;
230  // if at least one data have been sensed :
231  if(obs->sensedData.size() > 0)
232  {
233  appendObservation( obs );
234  }
235  }
236  catch(...)
237  {
238  m_state = ssError;
239  THROW_EXCEPTION("No observation received from the Phidget board!");
240  }
241 }
242 
243 /*-------------------------------------------------------------
244  getObservation
245 -------------------------------------------------------------*/
247 {
248 #if MRPT_HAS_PHIDGET
250  obs.sensorLabel = m_sensorLabel;
251  obs.minSensorDistance = m_minOfMinRanges;
252  obs.maxSensorDistance = m_maxOfMaxRanges;
253  obs.sensorConeApperture = DEG2RAD(2.0f); // TODO : Adapt to real sensor cone apperture.
254  obs.sensedData.clear();
255 
256  int sensorValue;
257  for(int i = 0 ; i < 8 ; i++)
258  {
259  if(m_sensorIsPlugged[i])
260  {
262  int err = CPhidgetInterfaceKit_getSensorValue(*((CPhidgetInterfaceKitHandle*)(m_carteInterfaceKit)), i, &sensorValue);
263  if(err>0)
264  {
265  string error("Error durring acquiering sensor value on channel : %d", i);
266  THROW_EXCEPTION(error);
267  }
268  switch (m_sensorType[i])
269  {
270  case EZ1 :
271  // TODO : find the conversion formula.
272  obsRange.sensedDistance = 1.0;
273  break;
274  case SHARP_30cm :
275  obsRange.sensedDistance = 2076./(static_cast<float>(sensorValue) - 11.);
276  break;
277  case SHARP_80cm :
278  obsRange.sensedDistance = 4800./(static_cast<float>(sensorValue) - 16.92);
279  break;
280  default :
281  obsRange.sensedDistance = -1;
282  break;
283  }
284 
285  obsRange.sensorID = i;
286  obsRange.sensorPose = m_sensorPoses[i]; // The pose of the IR sensor on the robot
287  obs.sensedData.push_back( obsRange );
288  }
289  }
290 #else
291  MRPT_UNUSED_PARAM(obs);
292 #endif
293 }
GLint GLint GLint GLint GLint GLint y
Definition: glew.h:1166
float read_float(const std::string &section, const std::string &name, float defaultValue, bool failIfNotFound=false) const
Declares a class derived from "CObservation" that encapsules a single range measurement, and associated parameters.
float minSensorDistance
The data members.
OBSERVATION_T::Ptr getObservation(mrpt::obs::CSensoryFramePtr &observations, mrpt::obs::CObservationPtr &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
float sensorConeApperture
Cone aperture of each ultrasonic beam, in radians.
mrpt::system::TTimeStamp BASE_IMPEXP getCurrentTime()
Returns the current (UTC) system time.
Definition: datetime.cpp:71
#define THROW_EXCEPTION(msg)
int read_int(const std::string &section, const std::string &name, int defaultValue, bool failIfNotFound=false) const
void getObservation(mrpt::obs::CObservationRange &outObservation)
This method tries to get a set of range measurements from the IR sensors.
This class allows loading and storing values and vectors of different types from a configuration text...
std::string read_string(const std::string &section, const std::string &name, const std::string &defaultValue, bool failIfNotFound=false) const
float sensedDistance
The measured range, in meters (or a value of 0 if there was no detected echo).
#define MRPT_UNUSED_PARAM(a)
Can be used to avoid "not used parameters" warnings from the compiler.
GLint GLint GLint GLint GLint x
Definition: glew.h:1166
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:21
#define DEG2RAD
#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.
mrpt::system::TTimeStamp timestamp
The associated UTC time-stamp. Where available, this should contain the accurate satellite-based time...
GLdouble GLdouble z
Definition: glew.h:1464
: An interface for the phidget Interface kit board (1018).
GLsizei const GLcharARB ** string
Definition: glew.h:3293
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:72
bool sectionExists(const std::string &section_name) const
Checks if a given section exists (name is case insensitive)
math::TPose3D sensorPose
The 6D position of the sensor on the robot.
void loadConfig_sensorSpecific(const mrpt::utils::CConfigFileBase &configSource, const std::string &iniSection)
See the class documentation at the top for expected parameters.
bool read_bool(const std::string &section, const std::string &name, bool defaultValue, bool failIfNotFound=false) const
void initialize()
Initialize the sensor according to the parameters previously read in the configuration file...
TMeasurementList sensedData
All the measurements.
GLdouble GLdouble GLdouble GLdouble GLdouble GLdouble f
Definition: glew.h:5092



Page generated by Doxygen 1.8.6 for MRPT 1.5.6 Git: 4c65e84 Tue Apr 24 08:18:17 2018 +0200 at mar abr 24 08:26:17 CEST 2018