MRPT  1.9.9
CIMUIntersense.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 <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 {
45  m_sensorLabel = "isenseIMU";
46 
47 #if MRPT_HAS_INTERSENSE
48  m_handles_ptr =
49  new ISD_TRACKER_HANDLE[ISD_MAX_TRACKERS](); // initialized to zeros
50 #else
52  "MRPT has been compiled with 'BUILD_INTERSENSE'=OFF, so this class "
53  "cannot be used.");
54 #endif
55 }
56 
57 /*-------------------------------------------------------------
58  ~CIMUIntersense
59 -------------------------------------------------------------*/
60 CIMUIntersense::~CIMUIntersense()
61 {
62 #if MRPT_HAS_INTERSENSE
63  ISD_CloseTracker(0); // close all the sensors
64  delete[] isense_handles;
65  m_handles_ptr = nullptr;
66 #endif
67 }
68 
69 /*-------------------------------------------------------------
70  doProcess
71 -------------------------------------------------------------*/
72 void CIMUIntersense::doProcess()
73 {
74 #if MRPT_HAS_INTERSENSE
75  if (m_state == ssError)
76  {
77  std::this_thread::sleep_for(200ms);
78  initialize();
79  }
80 
81  if (m_state == ssError) return;
82 
83  int n_data_ok = 0;
84  // add an observation for each sensor
85  for (int i = 0; i < m_nSensors; ++i)
86  {
87  ASSERT_(isense_handles[i] > 0)
88 
89  ISD_TRACKING_DATA_TYPE data;
90  Bool res_ok = ISD_GetTrackingData(isense_handles[i], &data);
91 
92  if (!res_ok) continue;
93 
94  // current (sensor) timestamp (in usecs)
95  // uint32_t nowUI = data.Station[0].TimeStampSeconds*10e6 +
96  // data.Station[0].TimeStampMicroSec;
97  float nowUI = data.Station[0].TimeStamp; // in seconds
98 
99  CObservationIMU::Ptr obs = std::make_shared<CObservationIMU>();
100 
101  // euler angles
102  obs->rawMeasurements[IMU_YAW] = DEG2RAD(data.Station[0].Euler[0]);
103  obs->dataIsPresent[IMU_YAW] = true;
104  obs->rawMeasurements[IMU_PITCH] = DEG2RAD(data.Station[0].Euler[1]);
105  obs->dataIsPresent[IMU_PITCH] = true;
106  obs->rawMeasurements[IMU_ROLL] = DEG2RAD(data.Station[0].Euler[2]);
107  obs->dataIsPresent[IMU_ROLL] = true;
108 
109  // angular velocity
110  obs->rawMeasurements[IMU_YAW_VEL] =
111  data.Station[0].AngularVelBodyFrame[0]; // rad/s
112  obs->dataIsPresent[IMU_YAW_VEL] = true;
113  obs->rawMeasurements[IMU_PITCH_VEL] =
114  data.Station[0].AngularVelBodyFrame[1]; // rad/s
115  obs->dataIsPresent[IMU_PITCH_VEL] = true;
116  obs->rawMeasurements[IMU_ROLL_VEL] =
117  data.Station[0].AngularVelBodyFrame[2]; // rad/s
118  obs->dataIsPresent[IMU_ROLL_VEL] = true;
119 
120  // angular velocity 2
121  obs->rawMeasurements[IMU_YAW_VEL_GLOBAL] =
122  data.Station[0].AngularVelNavFrame[0]; // rad/s
123  obs->dataIsPresent[IMU_YAW_VEL_GLOBAL] = true;
124  obs->rawMeasurements[IMU_PITCH_VEL_GLOBAL] =
125  data.Station[0].AngularVelNavFrame[1]; // rad/s
126  obs->dataIsPresent[IMU_PITCH_VEL_GLOBAL] = true;
127  obs->rawMeasurements[IMU_ROLL_VEL_GLOBAL] =
128  data.Station[0].AngularVelNavFrame[2]; // rad/s
129  obs->dataIsPresent[IMU_ROLL_VEL_GLOBAL] = true;
130 
131  // angular velocity 3 --> x,y,z velocity
132  obs->rawMeasurements[IMU_X_VEL] =
133  data.Station[0].VelocityNavFrame[0]; // m/s
134  obs->dataIsPresent[IMU_X_VEL] = true;
135  obs->rawMeasurements[IMU_Y_VEL] =
136  data.Station[0].VelocityNavFrame[1]; // m/s
137  obs->dataIsPresent[IMU_Y_VEL] = true;
138  obs->rawMeasurements[IMU_Z_VEL] =
139  data.Station[0].VelocityNavFrame[2]; // m/s
140  obs->dataIsPresent[IMU_Z_VEL] = true;
141 
142  // angular acceleration: global coords
143  obs->rawMeasurements[IMU_X_ACC_GLOBAL] =
144  data.Station[0].AccelNavFrame[0]; // m/s w/o gravity
145  obs->dataIsPresent[IMU_X_ACC_GLOBAL] = true;
146  obs->rawMeasurements[IMU_Y_ACC_GLOBAL] =
147  data.Station[0].AccelNavFrame[1]; // m/s w/o gravity
148  obs->dataIsPresent[IMU_Y_ACC_GLOBAL] = true;
149  obs->rawMeasurements[IMU_Z_ACC_GLOBAL] =
150  data.Station[0].AccelNavFrame[2]; // m/s w/o gravity
151  obs->dataIsPresent[IMU_Z_ACC_GLOBAL] = true;
152 
153  // angular acceleration 2: local coords
154  obs->rawMeasurements[IMU_X_ACC] = data.Station[0].AccelBodyFrame[0];
155  obs->dataIsPresent[IMU_X_ACC] = true;
156  obs->rawMeasurements[IMU_Y_ACC] = data.Station[0].AccelBodyFrame[1];
157  obs->dataIsPresent[IMU_Y_ACC] = true;
158  obs->rawMeasurements[IMU_Z_ACC] = data.Station[0].AccelBodyFrame[2];
159  obs->dataIsPresent[IMU_Z_ACC] = true;
160 
161  // position
162  // obs->rawMeasurements[IMU_X] =
163  // DEG2RAD(data.Station[0].Position[0]);
164  // obs->dataIsPresent[IMU_X] = true;
165  // obs->rawMeasurements[IMU_Y] =
166  // DEG2RAD(data.Station[0].Position[1]);
167  // obs->dataIsPresent[IMU_Y] = true;
168  // obs->rawMeasurements[IMU_Z] =
169  // DEG2RAD(data.Station[0].Position[2]);
170  // obs->dataIsPresent[IMU_Z] = true;
171 
172  // timestamp
173  // uint32_t AtUI = 0;
174  float AtUI = 0;
175  if (m_timeStartUI == 0)
176  {
177  m_timeStartUI = nowUI;
178  m_timeStartTT = mrpt::system::now();
179  }
180  else
181  AtUI = nowUI - m_timeStartUI;
182 
183  /* Board time is sec */
184  obs->timestamp = m_timeStartTT + std::chrono::microseconds(AtUI * 1e+6);
185 
186  // other stuff
187  obs->sensorPose = m_sensorPose;
188  obs->sensorLabel = m_sensorLabel + "_" + std::to_string(i);
189 
190  // add observation
191  appendObservation(obs);
192  m_toutCounter = 0;
193  n_data_ok++;
194  } // end-for
195 
196  if (n_data_ok == 0) // none of the sensors yielded data
197  m_toutCounter++;
198 
199  if (m_toutCounter > 3)
200  {
201  m_toutCounter = 0;
202  m_state = ssError;
203 
204  ISD_CloseTracker(0); // close all the sensors
205  }
206 #else
208  "MRPT has been compiled with 'BUILD_INTERSENSE'=OFF, so this class "
209  "cannot be used.");
210 #endif
211 }
212 
213 /*-------------------------------------------------------------
214  initialize
215 -------------------------------------------------------------*/
216 void CIMUIntersense::initialize()
217 {
218 #if MRPT_HAS_INTERSENSE
219  // ISD_TRACKER_HANDLE Trackers[ISD_MAX_TRACKERS];
220  DWORD openSuccess = FALSE;
221  cout << "Opening trackers... ";
222  openSuccess =
223  ISD_OpenAllTrackers((Hwnd) nullptr, isense_handles, FALSE, TRUE);
224  if (openSuccess < 1)
225  cout << "ERROR" << endl;
226  else
227  {
228  cout << "DONE" << endl;
229 
230  WORD numOpenTrackers = 0;
231  ISD_NumOpenTrackers(&numOpenTrackers);
232  cout << "Number of opened trackers: " << numOpenTrackers << endl;
233  vector<ISD_STATION_INFO_TYPE> station_info(numOpenTrackers);
234  for (int i = 0; i < ISD_MAX_TRACKERS; ++i)
235  {
236  if (isense_handles[i] > 0)
237  {
238  cout << "Retrieving configuration from sensor " << i << "...";
239  // get current configuration
240  // ISD_STATION_INFO_TYPE station;
241  Bool res_ok = ISD_GetStationConfig(
242  isense_handles[i],
243  &station_info[i], // & station,
244  i + 1 /*from 1 to ISD_MAX_TRACKERS*/, FALSE);
245 
246  if (!res_ok)
247  {
248  cout << " ERROR" << endl;
249  cout << "Sensor " << i
250  << " is working with default configuration!" << endl;
251  }
252  else
253  {
254  cout << " DONE" << endl;
255  // set custom configuration ...
256  // station.Sensitivity = m_sensitivity;
257  // station.Enhancement = m_enhancement;
258  // station.Prediction = m_prediction;
259  // station.TimeStamped = TRUE; // this must be TRUE to get
260  // timestamped data
261 
262  station_info[i].Sensitivity = m_sensitivity;
263  station_info[i].Enhancement = m_enhancement;
264  station_info[i].Prediction = m_prediction;
265  station_info[i].TimeStamped =
266  TRUE; // this must be TRUE to get timestamped data
267 
268  cout << "Setting configuration to sensor " << i << "...";
269  // .. and apply sensor configuration
270  res_ok = ISD_SetStationConfig(
271  isense_handles[i],
272  &station_info[i], // & station,
273  i + 1 /*from 1 to ISD_MAX_TRACKERS*/, FALSE);
274 
275  res_ok ? cout << " DONE" << endl : cout << " ERROR" << endl;
276 
277 #if 0 // set ring buffer to avoid data loss and start it:
278  // 180 samples is ~1 second at maximum data rate for wired devices
279  if( station_info[i].State == 1 /*station.State == 1*/ )
280  {
281  if( false && m_useBuffer )
282  {
283  ISD_RingBufferSetup(isense_handles[i],i,nullptr,180);
284  ISD_RingBufferStart(isense_handles[i],i);
285  }
286 
287  } // end-if
288 #endif
289  std::this_thread::sleep_for(500ms);
290  } // end-else
291  m_nSensors++;
292  } // end-if
293  } // end-for
294 
295  cout << "Found (and opened) " << m_nSensors << " sensors." << endl;
296  m_state = ssInitializing;
297  } // end-else
298 #else
300  "MRPT has been compiled with 'BUILD_INTERSENSE'=OFF, so this class "
301  "cannot be used.");
302 #endif
303 }
304 
305 /*-------------------------------------------------------------
306  loadConfig_sensorSpecific
307 -------------------------------------------------------------*/
308 void CIMUIntersense::loadConfig_sensorSpecific(
309  const mrpt::config::CConfigFileBase& configSource,
310  const std::string& iniSection)
311 {
312  m_sensorPose.setFromValues(
313  configSource.read_float(iniSection, "pose_x", 0, false),
314  configSource.read_float(iniSection, "pose_y", 0, false),
315  configSource.read_float(iniSection, "pose_z", 0, false),
316  DEG2RAD(configSource.read_float(iniSection, "pose_yaw", 0, false)),
317  DEG2RAD(configSource.read_float(iniSection, "pose_pitch", 0, false)),
318  DEG2RAD(configSource.read_float(iniSection, "pose_roll", 0, false)));
319 
320  m_sensitivity =
321  configSource.read_int(iniSection, "sensitivity", m_sensitivity, false);
322  m_enhancement =
323  configSource.read_int(iniSection, "enhancement", m_enhancement, false);
324  m_prediction =
325  configSource.read_int(iniSection, "prediction", m_prediction, false);
326  m_useBuffer =
327  configSource.read_bool(iniSection, "useBuffer", m_useBuffer, false);
328 
329  // dump parameters to console
330  cout << "---------------------------" << endl;
331  cout << "Intersense IMU parameters: " << endl;
332  cout << "---------------------------" << endl;
333  cout << "Sensitivity: " << m_sensitivity << endl;
334  cout << "Enhancement: " << m_enhancement << endl;
335  cout << "Prediction: " << m_prediction << endl;
336  cout << "Use buffer: " << m_useBuffer << endl;
337  cout << m_sensorPose << endl;
338  cout << "---------------------------" << endl << endl;
339 }
x-axis acceleration (global/navigation frame) (m/sec2)
z-axis acceleration (global/navigation frame) (m/sec2)
yaw angular velocity (global/navigation frame) (rad/sec)
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
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:86
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:120
pitch angular velocity (local/vehicle frame) (rad/sec)
This class allows loading and storing values and vectors of different types from a configuration text...
This namespace contains representation of robot actions and observations.
pitch angular velocity (global/navigation frame) (rad/sec)
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:
bool read_bool(const std::string &section, const std::string &name, bool defaultValue, bool failIfNotFound=false) const
#define FALSE
Definition: xmlParser.h:230
y-axis acceleration (global/navigation frame) (m/sec2)
#define isense_handles
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:30
orientation yaw absolute value (global/navigation frame) (rad)
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:233
GLsizei GLsizei GLenum GLenum const GLvoid * data
Definition: glext.h:3550
x-axis acceleration (local/vehicle frame) (m/sec2)
z-axis velocity (global/navigation frame) (m/sec)



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 8fe78517f Sun Jul 14 19:43:28 2019 +0200 at dom jul 14 20:00:11 CEST 2019