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 }
os.h
mrpt::obs::CObservationRange::TMeasurement
Definition: CObservationRange.h:42
mrpt::hwdrivers::CGenericSensor::ssWorking
@ ssWorking
Definition: CGenericSensor.h:87
mrpt::hwdrivers
Contains classes for various device interfaces.
Definition: C2DRangeFinderAbstract.h:19
mrpt::hwdrivers::UNPLUGGED
@ UNPLUGGED
Definition: CPhidgetInterfaceKitProximitySensors.h:98
mrpt::obs::gnss::roll
double roll
Definition: gnss_messages_novatel.h:306
mrpt::hwdrivers::CGenericSensor::ssError
@ ssError
Definition: CGenericSensor.h:88
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: BaseAppDataSource.h:15
mrpt::hwdrivers::CPhidgetInterfaceKitProximitySensors
: An interface for the phidget Interface kit board (1018).
Definition: CPhidgetInterfaceKitProximitySensors.h:101
THROW_EXCEPTION
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
mrpt::obs::CObservationRange::TMeasurement::sensedDistance
float sensedDistance
The measured range, in meters (or a value of 0 if there was no detected echo).
Definition: CObservationRange.h:57
mrpt::obs
This namespace contains representation of robot actions and observations.
Definition: CParticleFilter.h:17
mrpt::hwdrivers::SHARP_80cm
@ SHARP_80cm
Definition: CPhidgetInterfaceKitProximitySensors.h:96
mrpt::obs::gnss::error
uint32_t error
Definition: gnss_messages_novatel.h:407
CConfigFileBase.h
mrpt::obs::CObservationRange::TMeasurement::sensorID
uint16_t sensorID
Some kind of sensor ID which identifies it on the bus (if applicable, 0 otherwise)
Definition: CObservationRange.h:48
mrpt::obs::CObservationRange::Ptr
std::shared_ptr< mrpt::obs ::CObservationRange > Ptr
Definition: CObservationRange.h:29
mrpt::hwdrivers::SHARP_30cm
@ SHARP_30cm
Definition: CPhidgetInterfaceKitProximitySensors.h:95
mrpt::config::CConfigFileBase
This class allows loading and storing values and vectors of different types from a configuration text...
Definition: config/CConfigFileBase.h:44
mrpt::poses::CPose3D
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
CPhidgetInterfaceKitProximitySensors.h
mrpt::hwdrivers::CPhidgetInterfaceKitProximitySensors::~CPhidgetInterfaceKitProximitySensors
~CPhidgetInterfaceKitProximitySensors() override
Destructor.
Definition: CPhidgetInterfaceKitProximitySensors.cpp:236
mrpt::d2f
float d2f(const double d)
shortcut for static_cast<float>(double)
Definition: core/include/mrpt/core/bits_math.h:189
mrpt::obs::gnss::pitch
double pitch
Definition: gnss_messages_novatel.h:306
mrpt::DEG2RAD
constexpr double DEG2RAD(const double x)
Degrees to radians
Definition: core/include/mrpt/core/bits_math.h:47
mrpt::obs::CObservationRange::TMeasurement::sensorPose
math::TPose3D sensorPose
The 6D position of the sensor on the robot.
Definition: CObservationRange.h:52
IMPLEMENTS_GENERIC_SENSOR
#define IMPLEMENTS_GENERIC_SENSOR(class_name, NameSpace)
This must be inserted in all CGenericSensor classes implementation files:
Definition: CGenericSensor.h:314
mrpt::system::COutputLogger
Versatile class for consistent logging and management of output messages.
Definition: system/COutputLogger.h:117
mrpt::obs::CObservationRange
Declares a class derived from "CObservation" that encapsules a single range measurement,...
Definition: CObservationRange.h:27
mrpt::hwdrivers::CPhidgetInterfaceKitProximitySensors::getObservation
void getObservation(mrpt::obs::CObservationRange &outObservation)
This method tries to get a set of range measurements from the IR sensors.
Definition: CPhidgetInterfaceKitProximitySensors.cpp:274
mrpt::system::getCurrentTime
mrpt::system::TTimeStamp getCurrentTime()
Returns the current (UTC) system time.
Definition: datetime.h:82
mrpt::hwdrivers::CPhidgetInterfaceKitProximitySensors::loadConfig_sensorSpecific
void loadConfig_sensorSpecific(const mrpt::config::CConfigFileBase &configSource, const std::string &iniSection) override
See the class documentation at the top for expected parameters.
Definition: CPhidgetInterfaceKitProximitySensors.cpp:58
display
void display()
Definition: vision_stereo_rectify/test.cpp:32
mrpt::hwdrivers::CPhidgetInterfaceKitProximitySensors::initialize
void initialize() override
Initialize the sensor according to the parameters previously read in the configuration file.
Definition: CPhidgetInterfaceKitProximitySensors.cpp:190
mrpt::hwdrivers::CPhidgetInterfaceKitProximitySensors::doProcess
void doProcess() override
This method should be called periodically.
Definition: CPhidgetInterfaceKitProximitySensors.cpp:250
mrpt::format
std::string std::string format(std::string_view fmt, ARGS &&... args)
Definition: format.h:26
mrpt::hwdrivers::EZ1
@ EZ1
Definition: CPhidgetInterfaceKitProximitySensors.h:97
mrpt::system
Definition: backtrace.h:14
mrpt::obs::utils::getObservation
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
hwdrivers-precomp.h



Page generated by Doxygen 1.8.17 for MRPT 2.0.4 at Fri Jul 17 08:43:33 UTC 2020