Debian Codegraph @

libs/hwdrivers/src/CIMUIntersense.cpp

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