Main MRPT website > C++ reference for MRPT 1.9.9
CIMUXSens.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-2018, 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 
14 
15 #include <iostream>
16 #include <thread>
18 
19 using namespace mrpt::obs;
20 using namespace mrpt::hwdrivers;
21 using namespace std::literals;
22 
23 #if MRPT_HAS_xSENS_MT3
24 #include "xSens_MT3/cmt1.h"
25 #include "xSens_MT3/cmt2.h"
26 #include "xSens_MT3/cmt3.h"
27 #include "xSens_MT3/cmtdef.h"
28 #include "xSens_MT3/cmtmessage.h"
29 #include "xSens_MT3/cmtpacket.h"
30 #include "xSens_MT3/cmtscan.h"
33 #include "xSens_MT3/xsens_list.h"
34 #include "xSens_MT3/xsens_std.h"
35 #include "xSens_MT3/xsens_time.h"
36 #endif
37 
38 // Adaptors for the "void*" memory blocks:
39 #define cmt3 (*static_cast<xsens::Cmt3*>(m_cmt3_ptr))
40 #define deviceId (*static_cast<CmtDeviceId*>(m_deviceId_ptr))
41 
42 // Include libraries in linking:
43 #if MRPT_HAS_xSENS_MT3
44 #ifdef _WIN32
45 // WINDOWS:
46 #if defined(_MSC_VER)
47 #pragma comment(lib, "SetupAPI.lib")
48 #endif
49 #endif // _WIN32
50 #endif // MRPT_HAS_xSENS_MT3
51 
52 /*-------------------------------------------------------------
53  CIMUXSens
54 -------------------------------------------------------------*/
55 CIMUXSens::CIMUXSens()
56  : m_COMbauds(0),
57  m_com_port(),
58  m_timeStartUI(0),
59  m_timeStartTT(0),
60  m_sensorPose(),
61  m_cmt3_ptr(nullptr),
62  m_deviceId_ptr(nullptr),
63  m_toutCounter(0)
64 {
65  m_sensorLabel = "XSensMTi";
66 #if MRPT_HAS_xSENS_MT3
67  m_cmt3_ptr = new xsens::Cmt3[1];
68  m_deviceId_ptr = new CmtDeviceId[1];
69 
70 #else
72  "MRPT has been compiled with 'BUILD_XSENS_MT3'=OFF, so this class "
73  "cannot be used.");
74 #endif
75 }
76 
77 /*-------------------------------------------------------------
78  ~CIMUXSens
79 -------------------------------------------------------------*/
81 {
82 #if MRPT_HAS_xSENS_MT3
83  cmt3.closePort();
84 
85  delete[] & cmt3;
86  m_cmt3_ptr = nullptr;
87  delete[] & deviceId;
88  m_deviceId_ptr = nullptr;
89 #endif
90 }
91 
92 /*-------------------------------------------------------------
93  doProcess
94 -------------------------------------------------------------*/
96 {
97 #if MRPT_HAS_xSENS_MT3
98 
99  if (m_state == ssError)
100  {
101  std::this_thread::sleep_for(200ms);
102  initialize();
103  }
104 
105  if (m_state == ssError) return;
106 
108  unsigned int cont = 0;
109 
110  do
111  {
112  CmtTimeStamp nowUI; // ms
113 
114  xsens::Packet packet(1 /*NDevices*/, cmt3.isXm() /*Is Bus master*/);
115 
116  res = cmt3.waitForDataMessage(&packet);
117 
118  if (res == XRV_OK)
119  {
120  // Data properly collected
121  nowUI = packet.getRtc();
122  m_state = ssWorking;
123 
125  mrpt::make_aligned_shared<CObservationIMU>();
126 
127  // ANGLE MEASUREMENTS:
128  if (packet.containsOriEuler())
129  {
130  CmtEuler euler_data = packet.getOriEuler();
131 
132  obs->rawMeasurements[IMU_YAW] = DEG2RAD(euler_data.m_yaw);
133  obs->dataIsPresent[IMU_YAW] = true;
134  obs->rawMeasurements[IMU_PITCH] = DEG2RAD(euler_data.m_pitch);
135  obs->dataIsPresent[IMU_PITCH] = true;
136  obs->rawMeasurements[IMU_ROLL] = DEG2RAD(euler_data.m_roll);
137  obs->dataIsPresent[IMU_ROLL] = true;
138  }
139 
140  // ACCELEROMETERS MEASUREMENTS:
141  if (packet.containsCalAcc())
142  {
143  CmtVector acc_data = packet.getCalAcc(); // getRawAcc();
144 
145  obs->rawMeasurements[IMU_X_ACC] = acc_data.m_data[0];
146  obs->dataIsPresent[IMU_X_ACC] = true;
147  obs->rawMeasurements[IMU_Y_ACC] = acc_data.m_data[1];
148  obs->dataIsPresent[IMU_Y_ACC] = true;
149  obs->rawMeasurements[IMU_Z_ACC] = acc_data.m_data[2];
150  obs->dataIsPresent[IMU_Z_ACC] = true;
151  }
152 
153  // GYROSCOPES MEASUREMENTS:
154  if (packet.containsCalGyr())
155  {
156  CmtVector gir_data = packet.getCalGyr(); // getRawGyr();
157 
158  obs->rawMeasurements[IMU_YAW_VEL] = gir_data.m_data[2];
159  obs->dataIsPresent[IMU_YAW_VEL] = true;
160  obs->rawMeasurements[IMU_PITCH_VEL] = gir_data.m_data[1];
161  obs->dataIsPresent[IMU_PITCH_VEL] = true;
162  obs->rawMeasurements[IMU_ROLL_VEL] = gir_data.m_data[0];
163  obs->dataIsPresent[IMU_ROLL_VEL] = true;
164  }
165 
166  // TimeStamp
167  uint64_t AtUI = 0;
168  if (m_timeStartUI == 0)
169  {
170  m_timeStartUI = nowUI;
172  }
173  else
174  AtUI = nowUI - m_timeStartUI;
175 
176  double AtDO =
177  AtUI * 10000.0; // Difference in intervals of 100 nsecs
178  obs->timestamp = m_timeStartTT + AtDO;
179  obs->sensorPose = m_sensorPose;
180  obs->sensorLabel = m_sensorLabel;
181 
182  appendObservation(obs);
183  m_toutCounter = 0;
184 
185  } // end if XRV_OK
186 
187  if (res == XRV_TIMEOUT)
188  {
189  if (++m_toutCounter > 3)
190  {
191  m_toutCounter = 0;
192  m_state = ssError;
193  if (cmt3.isPortOpen()) cmt3.closePort();
194 
195  std::cerr << "[CIMUXSens::doProcess()] Error: No data "
196  "available [XRV_TIMEOUT]"
197  << std::endl;
198  }
199  } // end if XRV_TIMEOUT
200 
201  if (res == XRV_TIMEOUTNODATA)
202  {
203  // m_state = ssError;
204  // m_timeStartUI = 0;
205  // if( cmt3.isPortOpen() )
206  // cmt3.closePort();
207  // std::cerr << "[CIMUXSens::doProcess()] Error: No data
208  // available [XRV_TIMEOUTNODATA]" << std::endl;
209  } // end if XRV_TIMEOUTNODATA
210  } while (res == XRV_OK && cont++ < 30);
211 
212 #else
214  "MRPT has been compiled with 'BUILD_XSENS_MT3'=OFF, so this class "
215  "cannot be used.");
216 #endif
217 }
218 
219 /*-------------------------------------------------------------
220  lookForPort
221 -------------------------------------------------------------*/
223 {
224 #if MRPT_HAS_xSENS_MT3
225  uint32_t baudrate;
226  if (cmt3.getBaudrate(baudrate) == XRV_OK) return true;
227 
229  xsens::List<CmtPortInfo> portInfo;
230  unsigned long portCount = 0;
231  unsigned short mtCount = 0;
232 
233  if (m_com_port.empty())
234  { // Scan COM ports
235  std::cout << "Scanning for connected Xsens devices..." << std::endl;
236  xsens::cmtScanPorts(portInfo);
237  portCount = portInfo.length();
238  std::cout << "Done" << std::endl;
239  if (portCount == 0)
240  {
241  std::cout << "No xSens device found" << std::endl;
242  m_state = ssError;
243  return false;
244 
245  } // end if (error)
246  } // end if
247  else // Port defined by user in .ini file
248  {
249  CmtPortInfo pInfo;
250  pInfo.m_baudrate = m_COMbauds;
251  strcpy(
252  pInfo.m_portName,
253  m_com_port.c_str()); // m_portNr = (unsigned char)m_com_port;
254  portInfo.append(pInfo);
255  portCount++;
256  } // end else
257 
258  ASSERT_(portCount == 1);
259  std::cout << "Using COM port "
260  << portInfo[0].m_portName /*(long)portInfo[0].m_portNr*/ << " at "
261  << portInfo[0].m_baudrate << " baud" << std::endl;
262  std::cout << "Opening port..." << std::endl;
263  // open the port which the device is connected to and connect at the
264  // device's baudrate.
265  res = cmt3.openPort(portInfo[0].m_portName, portInfo[0].m_baudrate);
266  if (res != XRV_OK)
267  {
268  std::cerr << "COM Port could not be opened" << std::endl;
269  m_state = ssError;
270  return false;
271  }
272  std::cout << "done" << std::endl;
273 
274  // get the Mt sensor count.
275  std::cout
276  << "Retrieving MotionTracker count (excluding attached Xbus Master(s))"
277  << std::endl;
278  mtCount = cmt3.getMtCount();
279  std::cout << "MotionTracker count: " << mtCount << std::endl;
280 
281  ASSERT_(mtCount == 1);
282 
283  // retrieve the device IDs
284  std::cout << "Retrieving MotionTracker device ID" << std::endl;
285  res = cmt3.getDeviceId(mtCount, deviceId);
286  std::cout << "Device ID at busId 1: " << (long)deviceId
287  << std::endl; // printf("Device ID at busId 1: %08x\n",(long)
288  // deviceId);
289  if (res != XRV_OK)
290  {
291  std::cerr << "Device ID could not be gathered" << std::endl;
292  m_state = ssError;
293  return false;
294  }
295 
296  return true;
297 #else
298  return false;
299 #endif
300 } // end lookForPort
301 
302 /*-------------------------------------------------------------
303  initialize
304 -------------------------------------------------------------*/
306 {
307 #if MRPT_HAS_xSENS_MT3
308 
310 
311  if (cmt3.isPortOpen()) return;
312 
314 
315  // Search for the COM PORT and connect
316  if (!searchPortAndConnect())
317  {
318  m_state = ssError;
319  std::cerr << "Error Could not initialize the device" << std::endl;
320  return;
321  }
322 
323  std::cout << "xSens IMU detected and connected" << std::endl;
328 
329  // set the sensor to config state
330  res = cmt3.gotoConfig();
331  if (res != XRV_OK)
332  {
333  m_state = ssError; // EXIT_ON_ERROR(res,"gotoConfig");
334  std::cerr << "An error ocurred when setting the device to config mode"
335  << std::endl;
336  return;
337  }
338 
339  unsigned short sampleFreq;
340  sampleFreq = cmt3.getSampleFrequency();
341 
342  // set the device output mode for the device(s)
343  std::cout << "Configuring mode selection" << std::endl;
344  CmtDeviceMode deviceMode(mode, settings, sampleFreq);
345  res = cmt3.setDeviceMode(deviceMode, true, deviceId);
346  if (res != XRV_OK)
347  {
348  m_state = ssError; // EXIT_ON_ERROR(res,"setDeviceMode");
349  std::cerr << "An error ocurred when configuring the device"
350  << std::endl;
351  return;
352  }
353 
354  // start receiving data
355  res = cmt3.gotoMeasurement();
356  if (res != XRV_OK)
357  {
358  m_state = ssError; // EXIT_ON_ERROR(res,"gotoMeasurement");
359  std::cerr
360  << "An error ocurred when setting the device to measurement mode"
361  << std::endl;
362  return;
363  }
364 
365  std::cout << "Getting initial TimeStamp" << std::endl;
366  // Get initial TimeStamp
367  xsens::Packet packet(1 /*NDevices*/, cmt3.isXm() /*Is Bus master*/);
368  do
369  {
370  res = cmt3.waitForDataMessage(&packet);
371  if (res == XRV_OK)
372  {
373  m_timeStartUI = (uint64_t)packet.getRtc();
375  } // end if
376  } while (res != XRV_OK);
377 
378  std::cout << "Gathering data" << std::endl;
379  m_state = ssWorking;
380 
381 #else
383  "MRPT has been compiled with 'BUILD_XSENS_MT3'=OFF, so this class "
384  "cannot be used.");
385 #endif
386 }
387 
388 /*-------------------------------------------------------------
389  loadConfig_sensorSpecific
390 -------------------------------------------------------------*/
392  const mrpt::config::CConfigFileBase& configSource,
393  const std::string& iniSection)
394 {
396  configSource.read_float(iniSection, "pose_x", 0, false),
397  configSource.read_float(iniSection, "pose_y", 0, false),
398  configSource.read_float(iniSection, "pose_z", 0, false),
399  DEG2RAD(configSource.read_float(iniSection, "pose_yaw", 0, false)),
400  DEG2RAD(configSource.read_float(iniSection, "pose_pitch", 0, false)),
401  DEG2RAD(configSource.read_float(iniSection, "pose_roll", 0, false)));
402 
403  m_COMbauds =
404  configSource.read_int(iniSection, "baudRate", m_COMbauds, false);
405 
406 #ifdef _WIN32
407  m_com_port =
408  configSource.read_string(iniSection, "COM_port_WIN", m_com_port, false);
409 #else
410  m_com_port =
411  configSource.read_string(iniSection, "COM_port_LIN", m_com_port, false);
412 #endif
413 }
High-level communication class.
Definition: cmt3.h:39
Operation was performed successfully.
Definition: xsens_std.h:34
void append(const T &item)
Adds an item to the end of the list.
Definition: xsens_list.hpp:150
uint64_t CmtTimeStamp
Definition: cmtdef.h:1288
void appendObservation(const mrpt::serialization::CSerializable::Ptr &obj)
Like appendObservations() but for just one observation.
std::string read_string(const std::string &section, const std::string &name, const std::string &defaultValue, bool failIfNotFound=false) const
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:41
Structure for storing information about a serial port.
Definition: cmtdef.h:1168
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)
Definition: CIMUXSens.cpp:95
bool searchPortAndConnect()
Search the port where the sensor is located and connect to it.
Definition: CIMUXSens.cpp:222
mrpt::system::TTimeStamp now()
A shortcut for system::getCurrentTime.
Definition: datetime.h:75
Contains classes for various device interfaces.
float read_float(const std::string &section, 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)
Definition: cmtdef.h:1266
z-axis acceleration (local/vehicle frame) (m/sec2)
#define CMT_OUTPUTSETTINGS_TIMESTAMP_SAMPLECNT
Definition: cmtdef.h:625
#define CMT_OUTPUTMODE_CALIB
Definition: cmtdef.h:615
A class for interfacing XSens 3rd generation Inertial Measuring Units (IMUs), the "XSens MTi" model...
Definition: CIMUXSens.h:56
virtual ~CIMUXSens()
Destructor.
Definition: CIMUXSens.cpp:80
char m_portName[270]
The port name.
Definition: cmtdef.h:1177
mrpt::system::TTimeStamp m_timeStartTT
Definition: CIMUXSens.h:67
int read_int(const std::string &section, const std::string &name, int defaultValue, bool failIfNotFound=false) const
A structure containing MT data + timestamp and formatting information.
Definition: cmtpacket.h:24
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:113
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.
Definition: xsens_std.h:31
void initialize()
Turns on the xSens device and configure it for getting orientation data.
Definition: CIMUXSens.cpp:305
This namespace contains representation of robot actions and observations.
double m_roll
The roll (rotation around x-axis / back-front-line)
Definition: cmtdef.h:1264
uint32_t m_baudrate
The baudrate at which an Xsens device was detected.
Definition: cmtdef.h:1171
GLsizei const GLchar ** string
Definition: glext.h:4101
#define IMPLEMENTS_GENERIC_SENSOR(class_name, NameSpace)
This must be inserted in all CGenericSensor classes implementation files:
#define CMT_OUTPUTSETTINGS_ORIENTMODE_EULER
Definition: cmtdef.h:627
mrpt::poses::CPose3D m_sensorPose
Definition: CIMUXSens.h:69
double m_data[3]
Definition: cmtdef.h:1251
int m_COMbauds
This serial port will be attempted to be opened automatically when this class is first used to reques...
Definition: CIMUXSens.h:64
Dynamic list class.
Definition: xsens_list.h:64
GLint mode
Definition: glext.h:5669
unsigned __int64 uint64_t
Definition: rptypes.h:50
#define cmt3
Definition: CIMUXSens.cpp:39
unsigned int m_toutCounter
Timeout counter (for internal use only)
Definition: CIMUXSens.h:78
uint32_t length(void) const
Returns the number of items currently in the list.
Definition: xsens_list.h:159
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:239
uint32_t CmtOutputMode
An output mode bit-field.
Definition: cmtdef.h:902
void loadConfig_sensorSpecific(const mrpt::config::CConfigFileBase &configSource, const std::string &iniSection)
See the class documentation at the top for expected parameters.
Definition: CIMUXSens.cpp:391
double m_yaw
The yaw (rotation around z-axis / down-up-line)
Definition: cmtdef.h:1268
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.
Definition: cmtscan.cpp:133
GLuint res
Definition: glext.h:7268
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.
Definition: os.cpp:297
A structure for storing device modes.
Definition: cmtdef.h:1004
roll angular velocity (local/vehicle frame) (rad/sec)
unsigned __int32 uint32_t
Definition: rptypes.h:47
#define CMT_OUTPUTMODE_ORIENT
Definition: cmtdef.h:616
#define deviceId
Definition: CIMUXSens.cpp:40
x-axis acceleration (local/vehicle frame) (m/sec2)
uint32_t CmtDeviceId
The type of a Device Id.
Definition: cmtdef.h:854
#define CMT_OUTPUTSETTINGS_CALIBMODE_ACCGYR
Definition: cmtdef.h:631
uint64_t CmtOutputSettings
An output settings bit-field.
Definition: cmtdef.h:904



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019