Main MRPT website > C++ reference for MRPT 1.9.9
CIMUIntersense.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 
14 #include <iostream>
15 
17 
18 using namespace mrpt::obs;
19 using namespace mrpt::hwdrivers;
20 using namespace std;
21 
22 #if MRPT_HAS_INTERSENSE
23 #include "isense/isense.h"
24 #endif
25 
26 // Adaptors for the "void*" memory blocks:
27 #define isense_handles (static_cast<ISD_TRACKER_HANDLE*>(m_handles_ptr))
28 
29 // Include libraries in linking:
30 #if 0
31 #if MRPT_HAS_INTERSENSE
32 #ifdef _WIN32
33  // WINDOWS:
34 #if defined(_MSC_VER)
35 #pragma comment(lib, "isense.dll")
36 #endif
37 #endif // _WIN32
38 #endif // MRPT_HAS_INTERSENSE
39 #endif
40 /*-------------------------------------------------------------
41  CIMUIntersense
42 -------------------------------------------------------------*/
43 CIMUIntersense::CIMUIntersense()
44  : m_handles_ptr(nullptr),
45  m_timeStartUI(0),
46  m_timeStartTT(0),
47  m_sensorPose(),
48  m_nSensors(0),
49  m_sensitivity(10),
50  m_enhancement(2),
51  m_prediction(0),
52  m_useBuffer(false),
53  m_toutCounter(0)
54 {
55  m_sensorLabel = "isenseIMU";
56 
57 #if MRPT_HAS_INTERSENSE
59  new ISD_TRACKER_HANDLE[ISD_MAX_TRACKERS](); // initialized to zeros
60 #else
62  "MRPT has been compiled with 'BUILD_INTERSENSE'=OFF, so this class "
63  "cannot be used.");
64 #endif
65 }
66 
67 /*-------------------------------------------------------------
68  ~CIMUIntersense
69 -------------------------------------------------------------*/
71 {
72 #if MRPT_HAS_INTERSENSE
73  ISD_CloseTracker(0); // close all the sensors
74  delete[] isense_handles;
75  m_handles_ptr = nullptr;
76 #endif
77 }
78 
79 /*-------------------------------------------------------------
80  doProcess
81 -------------------------------------------------------------*/
83 {
84 #if MRPT_HAS_INTERSENSE
85  if (m_state == ssError)
86  {
87  std::this_thread::sleep_for(200ms);
88  initialize();
89  }
90 
91  if (m_state == ssError) return;
92 
93  int n_data_ok = 0;
94  // add an observation for each sensor
95  for (int i = 0; i < m_nSensors; ++i)
96  {
97  ASSERT_(isense_handles[i] > 0)
98 
99  ISD_TRACKING_DATA_TYPE data;
100  Bool res_ok = ISD_GetTrackingData(isense_handles[i], &data);
101 
102  if (!res_ok) continue;
103 
104  // current (sensor) timestamp (in usecs)
105  // uint32_t nowUI = data.Station[0].TimeStampSeconds*10e6 +
106  // data.Station[0].TimeStampMicroSec;
107  float nowUI = data.Station[0].TimeStamp; // in seconds
108 
109  CObservationIMU::Ptr obs = mrpt::make_aligned_shared<CObservationIMU>();
110 
111  // euler angles
112  obs->rawMeasurements[IMU_YAW] = DEG2RAD(data.Station[0].Euler[0]);
113  obs->dataIsPresent[IMU_YAW] = true;
114  obs->rawMeasurements[IMU_PITCH] = DEG2RAD(data.Station[0].Euler[1]);
115  obs->dataIsPresent[IMU_PITCH] = true;
116  obs->rawMeasurements[IMU_ROLL] = DEG2RAD(data.Station[0].Euler[2]);
117  obs->dataIsPresent[IMU_ROLL] = true;
118 
119  // angular velocity
120  obs->rawMeasurements[IMU_YAW_VEL] =
121  data.Station[0].AngularVelBodyFrame[0]; // rad/s
122  obs->dataIsPresent[IMU_YAW_VEL] = true;
123  obs->rawMeasurements[IMU_PITCH_VEL] =
124  data.Station[0].AngularVelBodyFrame[1]; // rad/s
125  obs->dataIsPresent[IMU_PITCH_VEL] = true;
126  obs->rawMeasurements[IMU_ROLL_VEL] =
127  data.Station[0].AngularVelBodyFrame[2]; // rad/s
128  obs->dataIsPresent[IMU_ROLL_VEL] = true;
129 
130  // angular velocity 2
131  obs->rawMeasurements[IMU_YAW_VEL_GLOBAL] =
132  data.Station[0].AngularVelNavFrame[0]; // rad/s
133  obs->dataIsPresent[IMU_YAW_VEL_GLOBAL] = true;
134  obs->rawMeasurements[IMU_PITCH_VEL_GLOBAL] =
135  data.Station[0].AngularVelNavFrame[1]; // rad/s
136  obs->dataIsPresent[IMU_PITCH_VEL_GLOBAL] = true;
137  obs->rawMeasurements[IMU_ROLL_VEL_GLOBAL] =
138  data.Station[0].AngularVelNavFrame[2]; // rad/s
139  obs->dataIsPresent[IMU_ROLL_VEL_GLOBAL] = true;
140 
141  // angular velocity 3 --> x,y,z velocity
142  obs->rawMeasurements[IMU_X_VEL] =
143  data.Station[0].VelocityNavFrame[0]; // m/s
144  obs->dataIsPresent[IMU_X_VEL] = true;
145  obs->rawMeasurements[IMU_Y_VEL] =
146  data.Station[0].VelocityNavFrame[1]; // m/s
147  obs->dataIsPresent[IMU_Y_VEL] = true;
148  obs->rawMeasurements[IMU_Z_VEL] =
149  data.Station[0].VelocityNavFrame[2]; // m/s
150  obs->dataIsPresent[IMU_Z_VEL] = true;
151 
152  // angular acceleration: global coords
153  obs->rawMeasurements[IMU_X_ACC_GLOBAL] =
154  data.Station[0].AccelNavFrame[0]; // m/s w/o gravity
155  obs->dataIsPresent[IMU_X_ACC_GLOBAL] = true;
156  obs->rawMeasurements[IMU_Y_ACC_GLOBAL] =
157  data.Station[0].AccelNavFrame[1]; // m/s w/o gravity
158  obs->dataIsPresent[IMU_Y_ACC_GLOBAL] = true;
159  obs->rawMeasurements[IMU_Z_ACC_GLOBAL] =
160  data.Station[0].AccelNavFrame[2]; // m/s w/o gravity
161  obs->dataIsPresent[IMU_Z_ACC_GLOBAL] = true;
162 
163  // angular acceleration 2: local coords
164  obs->rawMeasurements[IMU_X_ACC] = data.Station[0].AccelBodyFrame[0];
165  obs->dataIsPresent[IMU_X_ACC] = true;
166  obs->rawMeasurements[IMU_Y_ACC] = data.Station[0].AccelBodyFrame[1];
167  obs->dataIsPresent[IMU_Y_ACC] = true;
168  obs->rawMeasurements[IMU_Z_ACC] = data.Station[0].AccelBodyFrame[2];
169  obs->dataIsPresent[IMU_Z_ACC] = true;
170 
171  // position
172  // obs->rawMeasurements[IMU_X] =
173  // DEG2RAD(data.Station[0].Position[0]);
174  // obs->dataIsPresent[IMU_X] = true;
175  // obs->rawMeasurements[IMU_Y] =
176  // DEG2RAD(data.Station[0].Position[1]);
177  // obs->dataIsPresent[IMU_Y] = true;
178  // obs->rawMeasurements[IMU_Z] =
179  // DEG2RAD(data.Station[0].Position[2]);
180  // obs->dataIsPresent[IMU_Z] = true;
181 
182  // timestamp
183  // uint32_t AtUI = 0;
184  float AtUI = 0;
185  if (m_timeStartUI == 0)
186  {
187  m_timeStartUI = nowUI;
189  }
190  else
191  AtUI = nowUI - m_timeStartUI;
192 
193  // mrpt::system::TTimeStamp AtDO = mrpt::system::secondsToTimestamp(
194  // AtUI * 1e-6 /* Board time is usec */ );
196  mrpt::system::secondsToTimestamp(AtUI /* Board time is sec */);
197  obs->timestamp = m_timeStartTT + AtDO;
198 
199  // other stuff
200  obs->sensorPose = m_sensorPose;
201  obs->sensorLabel = m_sensorLabel + "_" + std::to_string(i);
202 
203  // add observation
204  appendObservation(obs);
205  m_toutCounter = 0;
206  n_data_ok++;
207  } // end-for
208 
209  if (n_data_ok == 0) // none of the sensors yielded data
210  m_toutCounter++;
211 
212  if (m_toutCounter > 3)
213  {
214  m_toutCounter = 0;
215  m_state = ssError;
216 
217  ISD_CloseTracker(0); // close all the sensors
218  }
219 #else
221  "MRPT has been compiled with 'BUILD_INTERSENSE'=OFF, so this class "
222  "cannot be used.");
223 #endif
224 }
225 
226 /*-------------------------------------------------------------
227  initialize
228 -------------------------------------------------------------*/
230 {
231 #if MRPT_HAS_INTERSENSE
232  // ISD_TRACKER_HANDLE Trackers[ISD_MAX_TRACKERS];
233  DWORD openSuccess = FALSE;
234  cout << "Opening trackers... ";
235  openSuccess =
236  ISD_OpenAllTrackers((Hwnd) nullptr, isense_handles, FALSE, TRUE);
237  if (openSuccess < 1)
238  cout << "ERROR" << endl;
239  else
240  {
241  cout << "DONE" << endl;
242 
243  WORD numOpenTrackers = 0;
244  ISD_NumOpenTrackers(&numOpenTrackers);
245  cout << "Number of opened trackers: " << numOpenTrackers << endl;
246  vector<ISD_STATION_INFO_TYPE> station_info(numOpenTrackers);
247  for (int i = 0; i < ISD_MAX_TRACKERS; ++i)
248  {
249  if (isense_handles[i] > 0)
250  {
251  cout << "Retrieving configuration from sensor " << i << "...";
252  // get current configuration
253  // ISD_STATION_INFO_TYPE station;
254  Bool res_ok = ISD_GetStationConfig(
255  isense_handles[i],
256  &station_info[i], // & station,
257  i + 1 /*from 1 to ISD_MAX_TRACKERS*/, FALSE);
258 
259  if (!res_ok)
260  {
261  cout << " ERROR" << endl;
262  cout << "Sensor " << i
263  << " is working with default configuration!" << endl;
264  }
265  else
266  {
267  cout << " DONE" << endl;
268  // set custom configuration ...
269  // station.Sensitivity = m_sensitivity;
270  // station.Enhancement = m_enhancement;
271  // station.Prediction = m_prediction;
272  // station.TimeStamped = TRUE; // this must be TRUE to get
273  // timestamped data
274 
275  station_info[i].Sensitivity = m_sensitivity;
276  station_info[i].Enhancement = m_enhancement;
277  station_info[i].Prediction = m_prediction;
278  station_info[i].TimeStamped =
279  TRUE; // this must be TRUE to get timestamped data
280 
281  cout << "Setting configuration to sensor " << i << "...";
282  // .. and apply sensor configuration
283  res_ok = ISD_SetStationConfig(
284  isense_handles[i],
285  &station_info[i], // & station,
286  i + 1 /*from 1 to ISD_MAX_TRACKERS*/, FALSE);
287 
288  res_ok ? cout << " DONE" << endl : cout << " ERROR" << endl;
289 
290 #if 0 // set ring buffer to avoid data loss and start it:
291  // 180 samples is ~1 second at maximum data rate for wired devices
292  if( station_info[i].State == 1 /*station.State == 1*/ )
293  {
294  if( false && m_useBuffer )
295  {
296  ISD_RingBufferSetup(isense_handles[i],i,nullptr,180);
297  ISD_RingBufferStart(isense_handles[i],i);
298  }
299 
300  } // end-if
301 #endif
302  std::this_thread::sleep_for(500ms);
303  } // end-else
304  m_nSensors++;
305  } // end-if
306  } // end-for
307 
308  cout << "Found (and opened) " << m_nSensors << " sensors." << endl;
310  } // end-else
311 #else
313  "MRPT has been compiled with 'BUILD_INTERSENSE'=OFF, so this class "
314  "cannot be used.");
315 #endif
316 }
317 
318 /*-------------------------------------------------------------
319  loadConfig_sensorSpecific
320 -------------------------------------------------------------*/
322  const mrpt::config::CConfigFileBase& configSource,
323  const std::string& iniSection)
324 {
326  configSource.read_float(iniSection, "pose_x", 0, false),
327  configSource.read_float(iniSection, "pose_y", 0, false),
328  configSource.read_float(iniSection, "pose_z", 0, false),
329  DEG2RAD(configSource.read_float(iniSection, "pose_yaw", 0, false)),
330  DEG2RAD(configSource.read_float(iniSection, "pose_pitch", 0, false)),
331  DEG2RAD(configSource.read_float(iniSection, "pose_roll", 0, false)));
332 
333  m_sensitivity =
334  configSource.read_int(iniSection, "sensitivity", m_sensitivity, false);
335  m_enhancement =
336  configSource.read_int(iniSection, "enhancement", m_enhancement, false);
337  m_prediction =
338  configSource.read_int(iniSection, "prediction", m_prediction, false);
339  m_useBuffer =
340  configSource.read_bool(iniSection, "useBuffer", m_useBuffer, false);
341 
342  // dump parameters to console
343  cout << "---------------------------" << endl;
344  cout << "Intersense IMU parameters: " << endl;
345  cout << "---------------------------" << endl;
346  cout << "Sensitivity: " << m_sensitivity << endl;
347  cout << "Enhancement: " << m_enhancement << endl;
348  cout << "Prediction: " << m_prediction << endl;
349  cout << "Use buffer: " << m_useBuffer << endl;
350  cout << m_sensorPose << endl;
351  cout << "---------------------------" << endl << endl;
352 }
x-axis acceleration (global/navigation frame) (m/sec2)
z-axis acceleration (global/navigation frame) (m/sec2)
yaw angular velocity (global/navigation frame) (rad/sec)
unsigned int m_toutCounter
Timeout counter (for internal use only)
void appendObservation(const mrpt::serialization::CSerializable::Ptr &obj)
Like appendObservations() but for just one observation.
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:41
std::string m_sensorLabel
See CGenericSensor.
double DEG2RAD(const double x)
Degrees to radians.
orientation pitch absolute value (global/navigation frame) (rad)
A class for interfacing Intersense Inertial Measuring Units (IMUs).
mrpt::system::TTimeStamp now()
A shortcut for system::getCurrentTime.
Definition: datetime.h:75
Contains classes for various device interfaces.
float read_float(const std::string &section, const std::string &name, float defaultValue, bool failIfNotFound=false) const
y-axis acceleration (local/vehicle frame) (m/sec2)
STL namespace.
z-axis acceleration (local/vehicle frame) (m/sec2)
x-axis velocity (global/navigation frame) (m/sec)
int read_int(const std::string &section, const std::string &name, int defaultValue, bool failIfNotFound=false) const
roll angular velocity (global/navigation frame) (rad/sec)
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:113
pitch angular velocity (local/vehicle frame) (rad/sec)
This class allows loading and storing values and vectors of different types from a configuration text...
mrpt::poses::CPose3D m_sensorPose
This namespace contains representation of robot actions and observations.
pitch angular velocity (global/navigation frame) (rad/sec)
void initialize()
Turns on the iSense device and configure it for getting orientation data.
void doProcess()
This method will be invoked at a minimum rate of "process_rate" (Hz)
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:
virtual ~CIMUIntersense()
Destructor.
bool read_bool(const std::string &section, const std::string &name, bool defaultValue, bool failIfNotFound=false) const
#define FALSE
Definition: xmlParser.h:231
y-axis acceleration (global/navigation frame) (m/sec2)
void setFromValues(const double x0, const double y0, const double z0, const double yaw=0, const double pitch=0, const double roll=0)
Set the pose from a 3D position (meters) and yaw/pitch/roll angles (radians) - This method recomputes...
Definition: CPose3D.cpp:239
#define isense_handles
uint64_t TTimeStamp
A system independent time type, it holds the the number of 100-nanosecond intervals since January 1...
Definition: datetime.h:31
void loadConfig_sensorSpecific(const mrpt::config::CConfigFileBase &configSource, const std::string &iniSection)
See the class documentation at the top for expected parameters.
void * m_handles_ptr
Opaque pointer to specifid iSense IMU structure.
mrpt::system::TTimeStamp secondsToTimestamp(const double nSeconds)
Transform a time interval (in seconds) into TTimeStamp (e.g.
Definition: datetime.cpp:224
orientation yaw absolute value (global/navigation frame) (rad)
mrpt::system::TTimeStamp m_timeStartTT
y-axis velocity (global/navigation frame) (m/sec)
orientation roll absolute value (global/navigation frame) (rad)
yaw angular velocity (local/vehicle frame) (rad/sec)
roll angular velocity (local/vehicle frame) (rad/sec)
#define TRUE
Definition: xmlParser.h:234
GLsizei GLsizei GLenum GLenum const GLvoid * data
Definition: glext.h:3546
uint32_t m_timeStartUI
Timestamp management.
x-axis acceleration (local/vehicle frame) (m/sec2)
z-axis velocity (global/navigation frame) (m/sec)
std::string std::string to_string(T v)
Just like std::to_string(), but with an overloaded version for std::string arguments.
Definition: format.h:27



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019