23 #if MRPT_HAS_xSENS_MT3 39 #define cmt3 (*static_cast<xsens::Cmt3*>(m_cmt3_ptr)) 40 #define deviceId (*static_cast<CmtDeviceId*>(m_deviceId_ptr)) 43 #if MRPT_HAS_xSENS_MT3 47 #pragma comment(lib, "SetupAPI.lib") 50 #endif // MRPT_HAS_xSENS_MT3 55 CIMUXSens::CIMUXSens()
62 m_deviceId_ptr(nullptr),
66 #if MRPT_HAS_xSENS_MT3 72 "MRPT has been compiled with 'BUILD_XSENS_MT3'=OFF, so this class " 82 #if MRPT_HAS_xSENS_MT3 97 #if MRPT_HAS_xSENS_MT3 101 std::this_thread::sleep_for(200ms);
108 unsigned int cont = 0;
116 res =
cmt3.waitForDataMessage(&packet);
121 nowUI = packet.getRtc();
125 mrpt::make_aligned_shared<CObservationIMU>();
128 if (packet.containsOriEuler())
130 CmtEuler euler_data = packet.getOriEuler();
133 obs->dataIsPresent[
IMU_YAW] =
true;
137 obs->dataIsPresent[
IMU_ROLL] =
true;
141 if (packet.containsCalAcc())
154 if (packet.containsCalGyr())
176 obs->timestamp =
m_timeStartTT + std::chrono::milliseconds(AtUI);
191 if (
cmt3.isPortOpen())
cmt3.closePort();
193 std::cerr <<
"[CIMUXSens::doProcess()] Error: No data " 194 "available [XRV_TIMEOUT]" 212 "MRPT has been compiled with 'BUILD_XSENS_MT3'=OFF, so this class " 222 #if MRPT_HAS_xSENS_MT3 224 if (
cmt3.getBaudrate(baudrate) ==
XRV_OK)
return true;
228 unsigned long portCount = 0;
229 unsigned short mtCount = 0;
233 std::cout <<
"Scanning for connected Xsens devices..." << std::endl;
235 portCount = portInfo.
length();
236 std::cout <<
"Done" << std::endl;
239 std::cout <<
"No xSens device found" << std::endl;
257 std::cout <<
"Using COM port " 258 << portInfo[0].m_portName <<
" at " 259 << portInfo[0].m_baudrate <<
" baud" << std::endl;
260 std::cout <<
"Opening port..." << std::endl;
263 res =
cmt3.openPort(portInfo[0].m_portName, portInfo[0].m_baudrate);
266 std::cerr <<
"COM Port could not be opened" << std::endl;
270 std::cout <<
"done" << std::endl;
274 <<
"Retrieving MotionTracker count (excluding attached Xbus Master(s))" 276 mtCount =
cmt3.getMtCount();
277 std::cout <<
"MotionTracker count: " << mtCount << std::endl;
282 std::cout <<
"Retrieving MotionTracker device ID" << std::endl;
284 std::cout <<
"Device ID at busId 1: " << (long)
deviceId 289 std::cerr <<
"Device ID could not be gathered" << std::endl;
305 #if MRPT_HAS_xSENS_MT3 309 if (
cmt3.isPortOpen())
return;
317 std::cerr <<
"Error Could not initialize the device" << std::endl;
321 std::cout <<
"xSens IMU detected and connected" << std::endl;
332 std::cerr <<
"An error ocurred when setting the device to config mode" 337 unsigned short sampleFreq;
338 sampleFreq =
cmt3.getSampleFrequency();
341 std::cout <<
"Configuring mode selection" << std::endl;
347 std::cerr <<
"An error ocurred when configuring the device" 358 <<
"An error ocurred when setting the device to measurement mode" 363 std::cout <<
"Getting initial TimeStamp" << std::endl;
368 res =
cmt3.waitForDataMessage(&packet);
376 std::cout <<
"Gathering data" << std::endl;
381 "MRPT has been compiled with 'BUILD_XSENS_MT3'=OFF, so this class " 394 configSource.
read_float(iniSection,
"pose_x", 0,
false),
395 configSource.
read_float(iniSection,
"pose_y", 0,
false),
396 configSource.
read_float(iniSection,
"pose_z", 0,
false),
High-level communication class.
Operation was performed successfully.
void append(const T &item)
Adds an item to the end of the list.
void appendObservation(const mrpt::serialization::CSerializable::Ptr &obj)
Like appendObservations() but for just one observation.
std::string read_string(const std::string §ion, const std::string &name, const std::string &defaultValue, bool failIfNotFound=false) const
#define THROW_EXCEPTION(msg)
Structure for storing information about a serial port.
std::string m_sensorLabel
See CGenericSensor.
double DEG2RAD(const double x)
Degrees to radians.
orientation pitch absolute value (global/navigation frame) (rad)
void doProcess()
This method will be invoked at a minimum rate of "process_rate" (Hz)
bool searchPortAndConnect()
Search the port where the sensor is located and connect to it.
mrpt::system::TTimeStamp now()
A shortcut for system::getCurrentTime.
Contains classes for various device interfaces.
float read_float(const std::string §ion, const std::string &name, float defaultValue, bool failIfNotFound=false) const
y-axis acceleration (local/vehicle frame) (m/sec2)
double m_pitch
The pitch (rotation around y-axis / right-left-line)
z-axis acceleration (local/vehicle frame) (m/sec2)
#define CMT_OUTPUTSETTINGS_TIMESTAMP_SAMPLECNT
#define CMT_OUTPUTMODE_CALIB
A class for interfacing XSens 3rd generation Inertial Measuring Units (IMUs), the "XSens MTi" model...
virtual ~CIMUXSens()
Destructor.
char m_portName[270]
The port name.
mrpt::system::TTimeStamp m_timeStartTT
int read_int(const std::string §ion, const std::string &name, int defaultValue, bool failIfNotFound=false) const
A structure containing MT data + timestamp and formatting information.
#define ASSERT_(f)
Defines an assertion mechanism.
pitch angular velocity (local/vehicle frame) (rad/sec)
This class allows loading and storing values and vectors of different types from a configuration text...
XsensResultValue
Xsens return values.
void initialize()
Turns on the xSens device and configure it for getting orientation data.
This namespace contains representation of robot actions and observations.
double m_roll
The roll (rotation around x-axis / back-front-line)
uint32_t m_baudrate
The baudrate at which an Xsens device was detected.
GLsizei const GLchar ** string
#define IMPLEMENTS_GENERIC_SENSOR(class_name, NameSpace)
This must be inserted in all CGenericSensor classes implementation files:
#define CMT_OUTPUTSETTINGS_ORIENTMODE_EULER
mrpt::poses::CPose3D m_sensorPose
int m_COMbauds
This serial port will be attempted to be opened automatically when this class is first used to reques...
unsigned __int64 uint64_t
unsigned int m_toutCounter
Timeout counter (for internal use only)
uint32_t length(void) const
Returns the number of items currently in the list.
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...
uint32_t CmtOutputMode
An output mode bit-field.
void loadConfig_sensorSpecific(const mrpt::config::CConfigFileBase &configSource, const std::string &iniSection)
See the class documentation at the top for expected parameters.
double m_yaw
The yaw (rotation around z-axis / down-up-line)
orientation yaw absolute value (global/navigation frame) (rad)
bool cmtScanPorts(List< CmtPortInfo > &ports, uint32_t baudrate, uint32_t singleScanTimeout, uint32_t scanTries)
Scan COM ports for connected Xsens devices.
orientation roll absolute value (global/navigation frame) (rad)
yaw angular velocity (local/vehicle frame) (rad/sec)
char * strcpy(char *dest, size_t destSize, const char *source) noexcept
An OS-independent version of strcpy.
A structure for storing device modes.
roll angular velocity (local/vehicle frame) (rad/sec)
unsigned __int32 uint32_t
#define CMT_OUTPUTMODE_ORIENT
x-axis acceleration (local/vehicle frame) (m/sec2)
uint32_t CmtDeviceId
The type of a Device Id.
#define CMT_OUTPUTSETTINGS_CALIBMODE_ACCGYR
uint64_t CmtOutputSettings
An output settings bit-field.