MRPT  1.9.9
CIMUXSens.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2019, 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 
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() : m_com_port(), m_timeStartTT(), m_sensorPose()
56 {
57  m_sensorLabel = "XSensMTi";
58 #if MRPT_HAS_xSENS_MT3
59  m_cmt3_ptr = new xsens::Cmt3[1];
60  m_deviceId_ptr = new CmtDeviceId[1];
61 
62 #else
64  "MRPT has been compiled with 'BUILD_XSENS_MT3'=OFF, so this class "
65  "cannot be used.");
66 #endif
67 }
68 
69 /*-------------------------------------------------------------
70  ~CIMUXSens
71 -------------------------------------------------------------*/
73 {
74 #if MRPT_HAS_xSENS_MT3
75  cmt3.closePort();
76 
77  delete[] & cmt3;
78  m_cmt3_ptr = nullptr;
79  delete[] & deviceId;
80  m_deviceId_ptr = nullptr;
81 #endif
82 }
83 
84 /*-------------------------------------------------------------
85  doProcess
86 -------------------------------------------------------------*/
88 {
89 #if MRPT_HAS_xSENS_MT3
90 
91  if (m_state == ssError)
92  {
93  std::this_thread::sleep_for(200ms);
94  initialize();
95  }
96 
97  if (m_state == ssError) return;
98 
100  unsigned int cont = 0;
101 
102  do
103  {
104  CmtTimeStamp nowUI; // ms
105 
106  xsens::Packet packet(1 /*NDevices*/, cmt3.isXm() /*Is Bus master*/);
107 
108  res = cmt3.waitForDataMessage(&packet);
109 
110  if (res == XRV_OK)
111  {
112  // Data properly collected
113  nowUI = packet.getRtc();
114  m_state = ssWorking;
115 
116  CObservationIMU::Ptr obs = std::make_shared<CObservationIMU>();
117 
118  // ANGLE MEASUREMENTS:
119  if (packet.containsOriEuler())
120  {
121  CmtEuler euler_data = packet.getOriEuler();
122 
123  obs->rawMeasurements[IMU_YAW] = DEG2RAD(euler_data.m_yaw);
124  obs->dataIsPresent[IMU_YAW] = true;
125  obs->rawMeasurements[IMU_PITCH] = DEG2RAD(euler_data.m_pitch);
126  obs->dataIsPresent[IMU_PITCH] = true;
127  obs->rawMeasurements[IMU_ROLL] = DEG2RAD(euler_data.m_roll);
128  obs->dataIsPresent[IMU_ROLL] = true;
129  }
130 
131  // ACCELEROMETERS MEASUREMENTS:
132  if (packet.containsCalAcc())
133  {
134  CmtVector acc_data = packet.getCalAcc(); // getRawAcc();
135 
136  obs->rawMeasurements[IMU_X_ACC] = acc_data.m_data[0];
137  obs->dataIsPresent[IMU_X_ACC] = true;
138  obs->rawMeasurements[IMU_Y_ACC] = acc_data.m_data[1];
139  obs->dataIsPresent[IMU_Y_ACC] = true;
140  obs->rawMeasurements[IMU_Z_ACC] = acc_data.m_data[2];
141  obs->dataIsPresent[IMU_Z_ACC] = true;
142  }
143 
144  // GYROSCOPES MEASUREMENTS:
145  if (packet.containsCalGyr())
146  {
147  CmtVector gir_data = packet.getCalGyr(); // getRawGyr();
148 
149  obs->rawMeasurements[IMU_YAW_VEL] = gir_data.m_data[2];
150  obs->dataIsPresent[IMU_YAW_VEL] = true;
151  obs->rawMeasurements[IMU_PITCH_VEL] = gir_data.m_data[1];
152  obs->dataIsPresent[IMU_PITCH_VEL] = true;
153  obs->rawMeasurements[IMU_ROLL_VEL] = gir_data.m_data[0];
154  obs->dataIsPresent[IMU_ROLL_VEL] = true;
155  }
156 
157  // TimeStamp
158  uint64_t AtUI = 0;
159  if (m_timeStartUI == 0)
160  {
161  m_timeStartUI = nowUI;
163  }
164  else
165  AtUI = nowUI - m_timeStartUI; // ms
166 
167  obs->timestamp = m_timeStartTT + std::chrono::milliseconds(AtUI);
168  obs->sensorPose = m_sensorPose;
169  obs->sensorLabel = m_sensorLabel;
170 
171  appendObservation(obs);
172  m_toutCounter = 0;
173 
174  } // end if XRV_OK
175 
176  if (res == XRV_TIMEOUT)
177  {
178  if (++m_toutCounter > 3)
179  {
180  m_toutCounter = 0;
181  m_state = ssError;
182  if (cmt3.isPortOpen()) cmt3.closePort();
183 
184  std::cerr << "[CIMUXSens::doProcess()] Error: No data "
185  "available [XRV_TIMEOUT]"
186  << std::endl;
187  }
188  } // end if XRV_TIMEOUT
189 
190  if (res == XRV_TIMEOUTNODATA)
191  {
192  // m_state = ssError;
193  // m_timeStartUI = 0;
194  // if( cmt3.isPortOpen() )
195  // cmt3.closePort();
196  // std::cerr << "[CIMUXSens::doProcess()] Error: No data
197  // available [XRV_TIMEOUTNODATA]" << std::endl;
198  } // end if XRV_TIMEOUTNODATA
199  } while (res == XRV_OK && cont++ < 30);
200 
201 #else
203  "MRPT has been compiled with 'BUILD_XSENS_MT3'=OFF, so this class "
204  "cannot be used.");
205 #endif
206 }
207 
208 /*-------------------------------------------------------------
209  lookForPort
210 -------------------------------------------------------------*/
212 {
213 #if MRPT_HAS_xSENS_MT3
214  uint32_t baudrate;
215  if (cmt3.getBaudrate(baudrate) == XRV_OK) return true;
216 
218  xsens::List<CmtPortInfo> portInfo;
219  unsigned long portCount = 0;
220  unsigned short mtCount = 0;
221 
222  if (m_com_port.empty())
223  { // Scan COM ports
224  std::cout << "Scanning for connected Xsens devices..." << std::endl;
225  xsens::cmtScanPorts(portInfo);
226  portCount = portInfo.length();
227  std::cout << "Done" << std::endl;
228  if (portCount == 0)
229  {
230  std::cout << "No xSens device found" << std::endl;
231  m_state = ssError;
232  return false;
233 
234  } // end if (error)
235  } // end if
236  else // Port defined by user in .ini file
237  {
238  CmtPortInfo pInfo;
239  pInfo.m_baudrate = m_COMbauds;
240  strcpy(
241  pInfo.m_portName,
242  m_com_port.c_str()); // m_portNr = (unsigned char)m_com_port;
243  portInfo.append(pInfo);
244  portCount++;
245  } // end else
246 
247  ASSERT_(portCount == 1);
248  std::cout << "Using COM port "
249  << portInfo[0].m_portName /*(long)portInfo[0].m_portNr*/ << " at "
250  << portInfo[0].m_baudrate << " baud" << std::endl;
251  std::cout << "Opening port..." << std::endl;
252  // open the port which the device is connected to and connect at the
253  // device's baudrate.
254  res = cmt3.openPort(portInfo[0].m_portName, portInfo[0].m_baudrate);
255  if (res != XRV_OK)
256  {
257  std::cerr << "COM Port could not be opened" << std::endl;
258  m_state = ssError;
259  return false;
260  }
261  std::cout << "done" << std::endl;
262 
263  // get the Mt sensor count.
264  std::cout
265  << "Retrieving MotionTracker count (excluding attached Xbus Master(s))"
266  << std::endl;
267  mtCount = cmt3.getMtCount();
268  std::cout << "MotionTracker count: " << mtCount << std::endl;
269 
270  ASSERT_(mtCount == 1);
271 
272  // retrieve the device IDs
273  std::cout << "Retrieving MotionTracker device ID" << std::endl;
274  res = cmt3.getDeviceId(mtCount, deviceId);
275  std::cout << "Device ID at busId 1: " << (long)deviceId
276  << std::endl; // printf("Device ID at busId 1: %08x\n",(long)
277  // deviceId);
278  if (res != XRV_OK)
279  {
280  std::cerr << "Device ID could not be gathered" << std::endl;
281  m_state = ssError;
282  return false;
283  }
284 
285  return true;
286 #else
287  return false;
288 #endif
289 } // end lookForPort
290 
291 /*-------------------------------------------------------------
292  initialize
293 -------------------------------------------------------------*/
295 {
296 #if MRPT_HAS_xSENS_MT3
297 
299 
300  if (cmt3.isPortOpen()) return;
301 
303 
304  // Search for the COM PORT and connect
305  if (!searchPortAndConnect())
306  {
307  m_state = ssError;
308  std::cerr << "Error Could not initialize the device" << std::endl;
309  return;
310  }
311 
312  std::cout << "xSens IMU detected and connected" << std::endl;
317 
318  // set the sensor to config state
319  res = cmt3.gotoConfig();
320  if (res != XRV_OK)
321  {
322  m_state = ssError; // EXIT_ON_ERROR(res,"gotoConfig");
323  std::cerr << "An error ocurred when setting the device to config mode"
324  << std::endl;
325  return;
326  }
327 
328  unsigned short sampleFreq;
329  sampleFreq = cmt3.getSampleFrequency();
330 
331  // set the device output mode for the device(s)
332  std::cout << "Configuring mode selection" << std::endl;
333  CmtDeviceMode deviceMode(mode, settings, sampleFreq);
334  res = cmt3.setDeviceMode(deviceMode, true, deviceId);
335  if (res != XRV_OK)
336  {
337  m_state = ssError; // EXIT_ON_ERROR(res,"setDeviceMode");
338  std::cerr << "An error ocurred when configuring the device"
339  << std::endl;
340  return;
341  }
342 
343  // start receiving data
344  res = cmt3.gotoMeasurement();
345  if (res != XRV_OK)
346  {
347  m_state = ssError; // EXIT_ON_ERROR(res,"gotoMeasurement");
348  std::cerr
349  << "An error ocurred when setting the device to measurement mode"
350  << std::endl;
351  return;
352  }
353 
354  std::cout << "Getting initial TimeStamp" << std::endl;
355  // Get initial TimeStamp
356  xsens::Packet packet(1 /*NDevices*/, cmt3.isXm() /*Is Bus master*/);
357  do
358  {
359  res = cmt3.waitForDataMessage(&packet);
360  if (res == XRV_OK)
361  {
362  m_timeStartUI = (uint64_t)packet.getRtc();
364  } // end if
365  } while (res != XRV_OK);
366 
367  std::cout << "Gathering data" << std::endl;
368  m_state = ssWorking;
369 
370 #else
372  "MRPT has been compiled with 'BUILD_XSENS_MT3'=OFF, so this class "
373  "cannot be used.");
374 #endif
375 }
376 
377 /*-------------------------------------------------------------
378  loadConfig_sensorSpecific
379 -------------------------------------------------------------*/
381  const mrpt::config::CConfigFileBase& configSource,
382  const std::string& iniSection)
383 {
385  configSource.read_float(iniSection, "pose_x", 0, false),
386  configSource.read_float(iniSection, "pose_y", 0, false),
387  configSource.read_float(iniSection, "pose_z", 0, false),
388  DEG2RAD(configSource.read_float(iniSection, "pose_yaw", 0, false)),
389  DEG2RAD(configSource.read_float(iniSection, "pose_pitch", 0, false)),
390  DEG2RAD(configSource.read_float(iniSection, "pose_roll", 0, false)));
391 
392  m_COMbauds =
393  configSource.read_int(iniSection, "baudRate", m_COMbauds, false);
394 
395 #ifdef _WIN32
396  m_com_port =
397  configSource.read_string(iniSection, "COM_port_WIN", m_com_port, false);
398 #else
399  m_com_port =
400  configSource.read_string(iniSection, "COM_port_LIN", m_com_port, false);
401 #endif
402 }
High-level communication class.
Definition: cmt3.h:39
Operation was performed successfully.
Definition: xsens_std.h:35
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
void initialize() override
Turns on the xSens device and configure it for getting orientation data.
Definition: CIMUXSens.cpp:294
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
~CIMUXSens() override
Destructor.
Definition: CIMUXSens.cpp:72
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 loadConfig_sensorSpecific(const mrpt::config::CConfigFileBase &configSource, const std::string &iniSection) override
See the class documentation at the top for expected parameters.
Definition: CIMUXSens.cpp:380
bool searchPortAndConnect()
Search the port where the sensor is located and connect to it.
Definition: CIMUXSens.cpp:211
mrpt::system::TTimeStamp now()
A shortcut for system::getCurrentTime.
Definition: datetime.h:86
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:53
char m_portName[270]
The port name.
Definition: cmtdef.h:1177
mrpt::system::TTimeStamp m_timeStartTT
Definition: CIMUXSens.h:64
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:120
pitch angular velocity (local/vehicle frame) (rad/sec)
This class allows loading and storing values and vectors of different types from a configuration text...
void doProcess() override
This method will be invoked at a minimum rate of "process_rate" (Hz)
Definition: CIMUXSens.cpp:87
XsensResultValue
Xsens return values.
Definition: xsens_std.h:31
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:4116
#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:66
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:61
Dynamic list class.
Definition: xsens_list.h:64
GLint mode
Definition: glext.h:5753
unsigned __int64 uint64_t
Definition: rptypes.h:53
#define cmt3
Definition: CIMUXSens.cpp:39
unsigned int m_toutCounter
Timeout counter (for internal use only)
Definition: CIMUXSens.h:75
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:255
uint32_t CmtOutputMode
An output mode bit-field.
Definition: cmtdef.h:902
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:134
GLuint res
Definition: glext.h:7385
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:299
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:50
#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: 1de0e027c Sat Sep 14 16:15:22 2019 +0200 at sáb sep 14 16:20:14 CEST 2019