/* +------------------------------------------------------------------------+ | Mobile Robot Programming Toolkit (MRPT) | | https://www.mrpt.org/ | | | | Copyright (c) 2005-2026, Individual contributors, see AUTHORS file | | See: https://www.mrpt.org/Authors - All rights reserved. | | Released under BSD License. See: https://www.mrpt.org/License | +------------------------------------------------------------------------+ */ #include "hwdrivers-precomp.h" // Precompiled headers // #include #include #include #include #include IMPLEMENTS_GENERIC_SENSOR(CIMUIntersense, mrpt::hwdrivers) using namespace mrpt::obs; using namespace mrpt::hwdrivers; using namespace std; #if MRPT_HAS_INTERSENSE #include "isense/isense.h" #endif // Adaptors for the "void*" memory blocks: #define isense_handles (static_cast(m_handles_ptr)) // Include libraries in linking: #if 0 #if MRPT_HAS_INTERSENSE #ifdef _WIN32 // WINDOWS: #if defined(_MSC_VER) #pragma comment(lib, "isense.dll") #endif #endif // _WIN32 #endif // MRPT_HAS_INTERSENSE #endif /*------------------------------------------------------------- CIMUIntersense -------------------------------------------------------------*/ CIMUIntersense::CIMUIntersense() { m_sensorLabel = "isenseIMU"; #if MRPT_HAS_INTERSENSE m_handles_ptr = new ISD_TRACKER_HANDLE[ISD_MAX_TRACKERS](); // initialized to zeros #else THROW_EXCEPTION( "MRPT has been compiled with 'BUILD_INTERSENSE'=OFF, so this class " "cannot be used."); #endif } /*------------------------------------------------------------- ~CIMUIntersense -------------------------------------------------------------*/ CIMUIntersense::~CIMUIntersense() { #if MRPT_HAS_INTERSENSE ISD_CloseTracker(0); // close all the sensors delete[] isense_handles; m_handles_ptr = nullptr; #endif } /*------------------------------------------------------------- doProcess -------------------------------------------------------------*/ void CIMUIntersense::doProcess() { #if MRPT_HAS_INTERSENSE if (m_state == ssError) { std::this_thread::sleep_for(200ms); initialize(); } if (m_state == ssError) return; int n_data_ok = 0; // add an observation for each sensor for (int i = 0; i < m_nSensors; ++i) { ASSERT_(isense_handles[i] > 0) ISD_TRACKING_DATA_TYPE data; Bool res_ok = ISD_GetTrackingData(isense_handles[i], &data); if (!res_ok) continue; // current (sensor) timestamp (in usecs) // uint32_t nowUI = data.Station[0].TimeStampSeconds*10e6 + // data.Station[0].TimeStampMicroSec; float nowUI = data.Station[0].TimeStamp; // in seconds CObservationIMU::Ptr obs = std::make_shared(); // euler angles obs->rawMeasurements[IMU_YAW] = DEG2RAD(data.Station[0].Euler[0]); obs->dataIsPresent[IMU_YAW] = true; obs->rawMeasurements[IMU_PITCH] = DEG2RAD(data.Station[0].Euler[1]); obs->dataIsPresent[IMU_PITCH] = true; obs->rawMeasurements[IMU_ROLL] = DEG2RAD(data.Station[0].Euler[2]); obs->dataIsPresent[IMU_ROLL] = true; // angular velocity obs->rawMeasurements[IMU_YAW_VEL] = data.Station[0].AngularVelBodyFrame[0]; // rad/s obs->dataIsPresent[IMU_YAW_VEL] = true; obs->rawMeasurements[IMU_PITCH_VEL] = data.Station[0].AngularVelBodyFrame[1]; // rad/s obs->dataIsPresent[IMU_PITCH_VEL] = true; obs->rawMeasurements[IMU_ROLL_VEL] = data.Station[0].AngularVelBodyFrame[2]; // rad/s obs->dataIsPresent[IMU_ROLL_VEL] = true; // angular velocity 2 obs->rawMeasurements[IMU_YAW_VEL_GLOBAL] = data.Station[0].AngularVelNavFrame[0]; // rad/s obs->dataIsPresent[IMU_YAW_VEL_GLOBAL] = true; obs->rawMeasurements[IMU_PITCH_VEL_GLOBAL] = data.Station[0].AngularVelNavFrame[1]; // rad/s obs->dataIsPresent[IMU_PITCH_VEL_GLOBAL] = true; obs->rawMeasurements[IMU_ROLL_VEL_GLOBAL] = data.Station[0].AngularVelNavFrame[2]; // rad/s obs->dataIsPresent[IMU_ROLL_VEL_GLOBAL] = true; // angular velocity 3 --> x,y,z velocity obs->rawMeasurements[IMU_X_VEL] = data.Station[0].VelocityNavFrame[0]; // m/s obs->dataIsPresent[IMU_X_VEL] = true; obs->rawMeasurements[IMU_Y_VEL] = data.Station[0].VelocityNavFrame[1]; // m/s obs->dataIsPresent[IMU_Y_VEL] = true; obs->rawMeasurements[IMU_Z_VEL] = data.Station[0].VelocityNavFrame[2]; // m/s obs->dataIsPresent[IMU_Z_VEL] = true; // angular acceleration: global coords obs->rawMeasurements[IMU_X_ACC_GLOBAL] = data.Station[0].AccelNavFrame[0]; // m/s w/o gravity obs->dataIsPresent[IMU_X_ACC_GLOBAL] = true; obs->rawMeasurements[IMU_Y_ACC_GLOBAL] = data.Station[0].AccelNavFrame[1]; // m/s w/o gravity obs->dataIsPresent[IMU_Y_ACC_GLOBAL] = true; obs->rawMeasurements[IMU_Z_ACC_GLOBAL] = data.Station[0].AccelNavFrame[2]; // m/s w/o gravity obs->dataIsPresent[IMU_Z_ACC_GLOBAL] = true; // angular acceleration 2: local coords obs->rawMeasurements[IMU_X_ACC] = data.Station[0].AccelBodyFrame[0]; obs->dataIsPresent[IMU_X_ACC] = true; obs->rawMeasurements[IMU_Y_ACC] = data.Station[0].AccelBodyFrame[1]; obs->dataIsPresent[IMU_Y_ACC] = true; obs->rawMeasurements[IMU_Z_ACC] = data.Station[0].AccelBodyFrame[2]; obs->dataIsPresent[IMU_Z_ACC] = true; // position // obs->rawMeasurements[IMU_X] = // DEG2RAD(data.Station[0].Position[0]); // obs->dataIsPresent[IMU_X] = true; // obs->rawMeasurements[IMU_Y] = // DEG2RAD(data.Station[0].Position[1]); // obs->dataIsPresent[IMU_Y] = true; // obs->rawMeasurements[IMU_Z] = // DEG2RAD(data.Station[0].Position[2]); // obs->dataIsPresent[IMU_Z] = true; // timestamp // uint32_t AtUI = 0; float AtUI = 0; if (m_timeStartUI == 0) { m_timeStartUI = nowUI; m_timeStartTT = mrpt::Clock::now(); } else AtUI = nowUI - m_timeStartUI; /* Board time is sec */ obs->timestamp = m_timeStartTT + std::chrono::microseconds(AtUI * 1e+6); // other stuff obs->sensorPose = m_sensorPose; obs->sensorLabel = m_sensorLabel + "_" + std::to_string(i); // add observation appendObservation(obs); m_toutCounter = 0; n_data_ok++; } // end-for if (n_data_ok == 0) // none of the sensors yielded data m_toutCounter++; if (m_toutCounter > 3) { m_toutCounter = 0; m_state = ssError; ISD_CloseTracker(0); // close all the sensors } #else THROW_EXCEPTION( "MRPT has been compiled with 'BUILD_INTERSENSE'=OFF, so this class " "cannot be used."); #endif } /*------------------------------------------------------------- initialize -------------------------------------------------------------*/ void CIMUIntersense::initialize() { #if MRPT_HAS_INTERSENSE // ISD_TRACKER_HANDLE Trackers[ISD_MAX_TRACKERS]; DWORD openSuccess = FALSE; cout << "Opening trackers... "; openSuccess = ISD_OpenAllTrackers((Hwnd) nullptr, isense_handles, FALSE, TRUE); if (openSuccess < 1) cout << "ERROR" << endl; else { cout << "DONE" << endl; WORD numOpenTrackers = 0; ISD_NumOpenTrackers(&numOpenTrackers); cout << "Number of opened trackers: " << numOpenTrackers << endl; vector station_info(numOpenTrackers); for (int i = 0; i < ISD_MAX_TRACKERS; ++i) { if (isense_handles[i] > 0) { cout << "Retrieving configuration from sensor " << i << "..."; // get current configuration // ISD_STATION_INFO_TYPE station; Bool res_ok = ISD_GetStationConfig( isense_handles[i], &station_info[i], // & station, i + 1 /*from 1 to ISD_MAX_TRACKERS*/, FALSE); if (!res_ok) { cout << " ERROR" << endl; cout << "Sensor " << i << " is working with default configuration!" << endl; } else { cout << " DONE" << endl; // set custom configuration ... // station.Sensitivity = m_sensitivity; // station.Enhancement = m_enhancement; // station.Prediction = m_prediction; // station.TimeStamped = TRUE; // this must be TRUE to get // timestamped data station_info[i].Sensitivity = m_sensitivity; station_info[i].Enhancement = m_enhancement; station_info[i].Prediction = m_prediction; station_info[i].TimeStamped = TRUE; // this must be TRUE to get timestamped data cout << "Setting configuration to sensor " << i << "..."; // .. and apply sensor configuration res_ok = ISD_SetStationConfig( isense_handles[i], &station_info[i], // & station, i + 1 /*from 1 to ISD_MAX_TRACKERS*/, FALSE); res_ok ? cout << " DONE" << endl : cout << " ERROR" << endl; #if 0 // set ring buffer to avoid data loss and start it: // 180 samples is ~1 second at maximum data rate for wired devices if( station_info[i].State == 1 /*station.State == 1*/ ) { if( false && m_useBuffer ) { ISD_RingBufferSetup(isense_handles[i],i,nullptr,180); ISD_RingBufferStart(isense_handles[i],i); } } // end-if #endif std::this_thread::sleep_for(500ms); } // end-else m_nSensors++; } // end-if } // end-for cout << "Found (and opened) " << m_nSensors << " sensors." << endl; m_state = ssInitializing; } // end-else #else THROW_EXCEPTION( "MRPT has been compiled with 'BUILD_INTERSENSE'=OFF, so this class " "cannot be used."); #endif } /*------------------------------------------------------------- loadConfig_sensorSpecific -------------------------------------------------------------*/ void CIMUIntersense::loadConfig_sensorSpecific( const mrpt::config::CConfigFileBase& configSource, const std::string& iniSection) { m_sensorPose.setFromValues( configSource.read_float(iniSection, "pose_x", 0, false), configSource.read_float(iniSection, "pose_y", 0, false), configSource.read_float(iniSection, "pose_z", 0, false), DEG2RAD(configSource.read_float(iniSection, "pose_yaw", 0, false)), DEG2RAD(configSource.read_float(iniSection, "pose_pitch", 0, false)), DEG2RAD(configSource.read_float(iniSection, "pose_roll", 0, false))); m_sensitivity = configSource.read_int(iniSection, "sensitivity", m_sensitivity, false); m_enhancement = configSource.read_int(iniSection, "enhancement", m_enhancement, false); m_prediction = configSource.read_int(iniSection, "prediction", m_prediction, false); m_useBuffer = configSource.read_bool(iniSection, "useBuffer", m_useBuffer, false); // dump parameters to console cout << "---------------------------" << endl; cout << "Intersense IMU parameters: " << endl; cout << "---------------------------" << endl; cout << "Sensitivity: " << m_sensitivity << endl; cout << "Enhancement: " << m_enhancement << endl; cout << "Prediction: " << m_prediction << endl; cout << "Use buffer: " << m_useBuffer << endl; cout << m_sensorPose << endl; cout << "---------------------------" << endl << endl; }