| 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 |
} |