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(),
59  m_timeStartTT(),
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; // ms
175 
176  obs->timestamp = m_timeStartTT + std::chrono::milliseconds(AtUI);
177  obs->sensorPose = m_sensorPose;
178  obs->sensorLabel = m_sensorLabel;
179 
180  appendObservation(obs);
181  m_toutCounter = 0;
182 
183  } // end if XRV_OK
184 
185  if (res == XRV_TIMEOUT)
186  {
187  if (++m_toutCounter > 3)
188  {
189  m_toutCounter = 0;
190  m_state = ssError;
191  if (cmt3.isPortOpen()) cmt3.closePort();
192 
193  std::cerr << "[CIMUXSens::doProcess()] Error: No data "
194  "available [XRV_TIMEOUT]"
195  << std::endl;
196  }
197  } // end if XRV_TIMEOUT
198 
199  if (res == XRV_TIMEOUTNODATA)
200  {
201  // m_state = ssError;
202  // m_timeStartUI = 0;
203  // if( cmt3.isPortOpen() )
204  // cmt3.closePort();
205  // std::cerr << "[CIMUXSens::doProcess()] Error: No data
206  // available [XRV_TIMEOUTNODATA]" << std::endl;
207  } // end if XRV_TIMEOUTNODATA
208  } while (res == XRV_OK && cont++ < 30);
209 
210 #else
212  "MRPT has been compiled with 'BUILD_XSENS_MT3'=OFF, so this class "
213  "cannot be used.");
214 #endif
215 }
216 
217 /*-------------------------------------------------------------
218  lookForPort
219 -------------------------------------------------------------*/
221 {
222 #if MRPT_HAS_xSENS_MT3
223  uint32_t baudrate;
224  if (cmt3.getBaudrate(baudrate) == XRV_OK) return true;
225 
227  xsens::List<CmtPortInfo> portInfo;
228  unsigned long portCount = 0;
229  unsigned short mtCount = 0;
230 
231  if (m_com_port.empty())
232  { // Scan COM ports
233  std::cout << "Scanning for connected Xsens devices..." << std::endl;
234  xsens::cmtScanPorts(portInfo);
235  portCount = portInfo.length();
236  std::cout << "Done" << std::endl;
237  if (portCount == 0)
238  {
239  std::cout << "No xSens device found" << std::endl;
240  m_state = ssError;
241  return false;
242 
243  } // end if (error)
244  } // end if
245  else // Port defined by user in .ini file
246  {
247  CmtPortInfo pInfo;
248  pInfo.m_baudrate = m_COMbauds;
249  strcpy(
250  pInfo.m_portName,
251  m_com_port.c_str()); // m_portNr = (unsigned char)m_com_port;
252  portInfo.append(pInfo);
253  portCount++;
254  } // end else
255 
256  ASSERT_(portCount == 1);
257  std::cout << "Using COM port "
258  << portInfo[0].m_portName /*(long)portInfo[0].m_portNr*/ << " at "
259  << portInfo[0].m_baudrate << " baud" << std::endl;
260  std::cout << "Opening port..." << std::endl;
261  // open the port which the device is connected to and connect at the
262  // device's baudrate.
263  res = cmt3.openPort(portInfo[0].m_portName, portInfo[0].m_baudrate);
264  if (res != XRV_OK)
265  {
266  std::cerr << "COM Port could not be opened" << std::endl;
267  m_state = ssError;
268  return false;
269  }
270  std::cout << "done" << std::endl;
271 
272  // get the Mt sensor count.
273  std::cout
274  << "Retrieving MotionTracker count (excluding attached Xbus Master(s))"
275  << std::endl;
276  mtCount = cmt3.getMtCount();
277  std::cout << "MotionTracker count: " << mtCount << std::endl;
278 
279  ASSERT_(mtCount == 1);
280 
281  // retrieve the device IDs
282  std::cout << "Retrieving MotionTracker device ID" << std::endl;
283  res = cmt3.getDeviceId(mtCount, deviceId);
284  std::cout << "Device ID at busId 1: " << (long)deviceId
285  << std::endl; // printf("Device ID at busId 1: %08x\n",(long)
286  // deviceId);
287  if (res != XRV_OK)
288  {
289  std::cerr << "Device ID could not be gathered" << std::endl;
290  m_state = ssError;
291  return false;
292  }
293 
294  return true;
295 #else
296  return false;
297 #endif
298 } // end lookForPort
299 
300 /*-------------------------------------------------------------
301  initialize
302 -------------------------------------------------------------*/
304 {
305 #if MRPT_HAS_xSENS_MT3
306 
308 
309  if (cmt3.isPortOpen()) return;
310 
312 
313  // Search for the COM PORT and connect
314  if (!searchPortAndConnect())
315  {
316  m_state = ssError;
317  std::cerr << "Error Could not initialize the device" << std::endl;
318  return;
319  }
320 
321  std::cout << "xSens IMU detected and connected" << std::endl;
326 
327  // set the sensor to config state
328  res = cmt3.gotoConfig();
329  if (res != XRV_OK)
330  {
331  m_state = ssError; // EXIT_ON_ERROR(res,"gotoConfig");
332  std::cerr << "An error ocurred when setting the device to config mode"
333  << std::endl;
334  return;
335  }
336 
337  unsigned short sampleFreq;
338  sampleFreq = cmt3.getSampleFrequency();
339 
340  // set the device output mode for the device(s)
341  std::cout << "Configuring mode selection" << std::endl;
342  CmtDeviceMode deviceMode(mode, settings, sampleFreq);
343  res = cmt3.setDeviceMode(deviceMode, true, deviceId);
344  if (res != XRV_OK)
345  {
346  m_state = ssError; // EXIT_ON_ERROR(res,"setDeviceMode");
347  std::cerr << "An error ocurred when configuring the device"
348  << std::endl;
349  return;
350  }
351 
352  // start receiving data
353  res = cmt3.gotoMeasurement();
354  if (res != XRV_OK)
355  {
356  m_state = ssError; // EXIT_ON_ERROR(res,"gotoMeasurement");
357  std::cerr
358  << "An error ocurred when setting the device to measurement mode"
359  << std::endl;
360  return;
361  }
362 
363  std::cout << "Getting initial TimeStamp" << std::endl;
364  // Get initial TimeStamp
365  xsens::Packet packet(1 /*NDevices*/, cmt3.isXm() /*Is Bus master*/);
366  do
367  {
368  res = cmt3.waitForDataMessage(&packet);
369  if (res == XRV_OK)
370  {
371  m_timeStartUI = (uint64_t)packet.getRtc();
373  } // end if
374  } while (res != XRV_OK);
375 
376  std::cout << "Gathering data" << std::endl;
377  m_state = ssWorking;
378 
379 #else
381  "MRPT has been compiled with 'BUILD_XSENS_MT3'=OFF, so this class "
382  "cannot be used.");
383 #endif
384 }
385 
386 /*-------------------------------------------------------------
387  loadConfig_sensorSpecific
388 -------------------------------------------------------------*/
390  const mrpt::config::CConfigFileBase& configSource,
391  const std::string& iniSection)
392 {
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),
397  DEG2RAD(configSource.read_float(iniSection, "pose_yaw", 0, false)),
398  DEG2RAD(configSource.read_float(iniSection, "pose_pitch", 0, false)),
399  DEG2RAD(configSource.read_float(iniSection, "pose_roll", 0, false)));
400 
401  m_COMbauds =
402  configSource.read_int(iniSection, "baudRate", m_COMbauds, false);
403 
404 #ifdef _WIN32
405  m_com_port =
406  configSource.read_string(iniSection, "COM_port_WIN", m_com_port, false);
407 #else
408  m_com_port =
409  configSource.read_string(iniSection, "COM_port_LIN", m_com_port, false);
410 #endif
411 }
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:220
mrpt::system::TTimeStamp now()
A shortcut for system::getCurrentTime.
Definition: datetime.h:87
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:54
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:65
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:303
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:67
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:62
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:76
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:389
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: 7d5e6d718 Fri Aug 24 01:51:28 2018 +0200 at lun nov 2 08:35:50 CET 2020