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