Main MRPT website > C++ reference for MRPT 1.5.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-2017, 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 
12 
13 #include <mrpt/system/threads.h>
16 
18 
19 using namespace mrpt::utils;
20 using namespace mrpt::obs;
21 using namespace mrpt::hwdrivers;
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 
39 // Adaptors for the "void*" memory blocks:
40 #define cmt3 (*static_cast<xsens::Cmt3*>(m_cmt3_ptr))
41 #define deviceId (*static_cast<CmtDeviceId*>(m_deviceId_ptr))
42 
43 
44 // Include libraries in linking:
45 #if MRPT_HAS_xSENS_MT3
46  #ifdef MRPT_OS_WINDOWS
47  // WINDOWS:
48  #if defined(_MSC_VER) || defined(__BORLANDC__)
49  #pragma comment (lib,"SetupAPI.lib")
50  #endif
51  #endif // MRPT_OS_WINDOWS
52 #endif // MRPT_HAS_xSENS_MT3
53 
54 /*-------------------------------------------------------------
55  CIMUXSens
56 -------------------------------------------------------------*/
57 CIMUXSens::CIMUXSens( ) :
58  m_COMbauds (0),
59  m_com_port (),
60  m_timeStartUI (0),
61  m_timeStartTT (0),
62  m_sensorPose (),
63  m_cmt3_ptr (NULL),
64  m_deviceId_ptr (NULL),
65  m_toutCounter (0)
66 {
67  m_sensorLabel = "XSensMTi";
68 #if MRPT_HAS_xSENS_MT3
69  m_cmt3_ptr = new xsens::Cmt3[1];
70  m_deviceId_ptr = new CmtDeviceId[1];
71 
72 #else
73  THROW_EXCEPTION("MRPT has been compiled with 'BUILD_XSENS_MT3'=OFF, so this class cannot be used.");
74 #endif
75 
76 }
77 
78 /*-------------------------------------------------------------
79  ~CIMUXSens
80 -------------------------------------------------------------*/
82 {
83 #if MRPT_HAS_xSENS_MT3
84  cmt3.closePort();
85 
86  delete[] &cmt3; m_cmt3_ptr= NULL;
87  delete[] &deviceId; m_deviceId_ptr = NULL;
88 #endif
89 }
90 
91 /*-------------------------------------------------------------
92  doProcess
93 -------------------------------------------------------------*/
95 {
96 #if MRPT_HAS_xSENS_MT3
97 
98  if(m_state == ssError)
99  {
100  mrpt::system::sleep(200);
101  initialize();
102  }
103 
104  if(m_state == ssError)
105  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 
124  CObservationIMUPtr obs = CObservationIMU::Create();
125 
126  // ANGLE MEASUREMENTS:
127  if ( packet.containsOriEuler() )
128  {
129  CmtEuler euler_data = packet.getOriEuler();
130 
131  obs->rawMeasurements[IMU_YAW] = DEG2RAD(euler_data.m_yaw);
132  obs->dataIsPresent[IMU_YAW] = true;
133  obs->rawMeasurements[IMU_PITCH] = DEG2RAD(euler_data.m_pitch);
134  obs->dataIsPresent[IMU_PITCH] = true;
135  obs->rawMeasurements[IMU_ROLL] = DEG2RAD(euler_data.m_roll);
136  obs->dataIsPresent[IMU_ROLL] = true;
137  }
138 
139  // ACCELEROMETERS MEASUREMENTS:
140  if ( packet.containsCalAcc())
141  {
142  CmtVector acc_data = packet.getCalAcc(); // getRawAcc();
143 
144  obs->rawMeasurements[IMU_X_ACC] = acc_data.m_data[0];
145  obs->dataIsPresent[IMU_X_ACC] = true;
146  obs->rawMeasurements[IMU_Y_ACC] = acc_data.m_data[1];
147  obs->dataIsPresent[IMU_Y_ACC] = true;
148  obs->rawMeasurements[IMU_Z_ACC] = acc_data.m_data[2];
149  obs->dataIsPresent[IMU_Z_ACC] = true;
150  }
151 
152  // GYROSCOPES MEASUREMENTS:
153  if ( packet.containsCalGyr())
154  {
155  CmtVector gir_data = packet.getCalGyr(); // getRawGyr();
156 
157  obs->rawMeasurements[IMU_YAW_VEL] = gir_data.m_data[2];
158  obs->dataIsPresent[IMU_YAW_VEL] = true;
159  obs->rawMeasurements[IMU_PITCH_VEL] = gir_data.m_data[1];
160  obs->dataIsPresent[IMU_PITCH_VEL] = true;
161  obs->rawMeasurements[IMU_ROLL_VEL] = gir_data.m_data[0];
162  obs->dataIsPresent[IMU_ROLL_VEL] = true;
163  }
164 
165  // TimeStamp
166  uint64_t AtUI = 0;
167  if( m_timeStartUI == 0 )
168  {
169  m_timeStartUI = nowUI;
171  }
172  else
173  AtUI = nowUI - m_timeStartUI;
174 
175  double AtDO = AtUI * 10000.0; // Difference in intervals of 100 nsecs
176  obs->timestamp = m_timeStartTT + AtDO;
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() )
192  cmt3.closePort();
193 
194  std::cerr << "[CIMUXSens::doProcess()] Error: No data available [XRV_TIMEOUT]" << std::endl;
195  }
196  } // end if XRV_TIMEOUT
197 
198  if(res == XRV_TIMEOUTNODATA)
199  {
200 // m_state = ssError;
201 // m_timeStartUI = 0;
202 // if( cmt3.isPortOpen() )
203 // cmt3.closePort();
204 // std::cerr << "[CIMUXSens::doProcess()] Error: No data available [XRV_TIMEOUTNODATA]" << std::endl;
205  } // end if XRV_TIMEOUTNODATA
206  } while( res == XRV_OK && cont++ < 30);
207 
208 #else
209  THROW_EXCEPTION("MRPT has been compiled with 'BUILD_XSENS_MT3'=OFF, so this class cannot be used.");
210 #endif
211 }
212 
213 /*-------------------------------------------------------------
214  lookForPort
215 -------------------------------------------------------------*/
217 {
218 #if MRPT_HAS_xSENS_MT3
219  uint32_t baudrate;
220  if(cmt3.getBaudrate(baudrate) == XRV_OK)
221  return true;
222 
224  xsens::List<CmtPortInfo> portInfo;
225  unsigned long portCount = 0;
226  unsigned short mtCount = 0;
227 
228  if( m_com_port.empty() ) { // Scan COM ports
229  std::cout << "Scanning for connected Xsens devices..." << std::endl;
230  xsens::cmtScanPorts(portInfo);
231  portCount = portInfo.length();
232  std::cout << "Done" << std::endl;
233  if (portCount == 0) {
234  std::cout << "No xSens device found" << std::endl;
235  m_state = ssError;
236  return false;
237 
238  } // end if (error)
239  } // end if
240  else // Port defined by user in .ini file
241  {
242  CmtPortInfo pInfo;
243  pInfo.m_baudrate = m_COMbauds;
244  strcpy( pInfo.m_portName, m_com_port.c_str()); //m_portNr = (unsigned char)m_com_port;
245  portInfo.append( pInfo );
246  portCount++;
247  } // end else
248 
249  ASSERT_(portCount == 1);
250  std::cout << "Using COM port " << portInfo[0].m_portName /*(long)portInfo[0].m_portNr*/ << " at " << 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 device's baudrate.
253  res = cmt3.openPort(portInfo[0].m_portName , portInfo[0].m_baudrate);
254  if (res != XRV_OK) {
255  std::cerr << "COM Port could not be opened" << std::endl;
256  m_state = ssError;
257  return false;
258  }
259  std::cout << "done" << std::endl;
260 
261  //get the Mt sensor count.
262  std::cout << "Retrieving MotionTracker count (excluding attached Xbus Master(s))" << std::endl;
263  mtCount = cmt3.getMtCount();
264  std::cout << "MotionTracker count: " << mtCount << std::endl;
265 
266  ASSERT_(mtCount == 1);
267 
268  // retrieve the device IDs
269  std::cout << "Retrieving MotionTracker device ID" << std::endl;
270  res = cmt3.getDeviceId(mtCount, deviceId);
271  std::cout << "Device ID at busId 1: " << (long) deviceId << std::endl; //printf("Device ID at busId 1: %08x\n",(long) deviceId);
272  if (res != XRV_OK) {
273  std::cerr << "Device ID could not be gathered" << std::endl;
274  m_state = ssError;
275  return false;
276  }
277 
278  return true;
279 #else
280  return false;
281 #endif
282 } // end lookForPort
283 
284 /*-------------------------------------------------------------
285  initialize
286 -------------------------------------------------------------*/
288 {
289 #if MRPT_HAS_xSENS_MT3
290 
292 
293  if(cmt3.isPortOpen())
294  return;
295 
297 
298  // Search for the COM PORT and connect
299  if(!searchPortAndConnect())
300  {
301  m_state = ssError;
302  std::cerr << "Error Could not initialize the device" << std::endl;
303  return;
304  }
305 
306  std::cout << "xSens IMU detected and connected" << std::endl;
309 
310  // set the sensor to config state
311  res = cmt3.gotoConfig();
312  if (res != XRV_OK) {
313  m_state = ssError; //EXIT_ON_ERROR(res,"gotoConfig");
314  std::cerr << "An error ocurred when setting the device to config mode" << std::endl;
315  return;
316  }
317 
318  unsigned short sampleFreq;
319  sampleFreq = cmt3.getSampleFrequency();
320 
321  // set the device output mode for the device(s)
322  std::cout << "Configuring mode selection" << std::endl;
323  CmtDeviceMode deviceMode(mode, settings, sampleFreq);
324  res = cmt3.setDeviceMode(deviceMode, true, deviceId);
325  if (res != XRV_OK) {
326  m_state = ssError; //EXIT_ON_ERROR(res,"setDeviceMode");
327  std::cerr << "An error ocurred when configuring the device" << std::endl;
328  return;
329  }
330 
331  // start receiving data
332  res = cmt3.gotoMeasurement();
333  if (res != XRV_OK) {
334  m_state = ssError; //EXIT_ON_ERROR(res,"gotoMeasurement");
335  std::cerr << "An error ocurred when setting the device to measurement mode" << std::endl;
336  return;
337  }
338 
339  std::cout << "Getting initial TimeStamp" << std::endl;
340  // Get initial TimeStamp
341  xsens::Packet packet(1/*NDevices*/,cmt3.isXm()/*Is Bus master*/);
342  do
343  {
344  res = cmt3.waitForDataMessage(&packet);
345  if( res == XRV_OK )
346  {
347  m_timeStartUI = (uint64_t)packet.getRtc();
349  } // end if
350  } while( res != XRV_OK );
351 
352  std::cout << "Gathering data" << std::endl;
353  m_state = ssWorking;
354 
355 #else
356  THROW_EXCEPTION("MRPT has been compiled with 'BUILD_XSENS_MT3'=OFF, so this class cannot be used.");
357 #endif
358 }
359 
360 /*-------------------------------------------------------------
361  loadConfig_sensorSpecific
362 -------------------------------------------------------------*/
364  const mrpt::utils::CConfigFileBase &configSource,
365  const std::string &iniSection )
366 {
368  configSource.read_float( iniSection, "pose_x", 0, false ),
369  configSource.read_float( iniSection, "pose_y", 0, false ),
370  configSource.read_float( iniSection, "pose_z", 0, false ),
371  DEG2RAD( configSource.read_float( iniSection, "pose_yaw", 0, false ) ),
372  DEG2RAD( configSource.read_float( iniSection, "pose_pitch", 0, false ) ),
373  DEG2RAD( configSource.read_float( iniSection, "pose_roll", 0, false ) ) );
374 
375  m_COMbauds = configSource.read_int(iniSection, "baudRate", m_COMbauds, false );
376 
377 #ifdef MRPT_OS_WINDOWS
378  m_com_port = configSource.read_string(iniSection, "COM_port_WIN", m_com_port, false );
379 #else
380  m_com_port = configSource.read_string(iniSection, "COM_port_LIN", m_com_port, false );
381 #endif
382 
383 
384 }
High-level communication class.
Definition: cmt3.h:36
Operation was performed successfully.
Definition: xsens_std.h:32
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:1188
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: zip.h:16
A timeout occurred.
Definition: xsens_std.h:60
float read_float(const std::string &section, const std::string &name, float defaultValue, bool failIfNotFound=false) const
Operation aborted because of no data read.
Definition: xsens_std.h:61
Structure for storing information about a serial port.
Definition: cmtdef.h:1097
std::string m_sensorLabel
See CGenericSensor.
#define THROW_EXCEPTION(msg)
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:94
bool searchPortAndConnect()
Search the port where the sensor is located and connect to it.
Definition: CIMUXSens.cpp:216
mrpt::system::TTimeStamp now()
A shortcut for system::getCurrentTime.
Definition: datetime.h:70
Contains classes for various device interfaces.
std::string read_string(const std::string &section, const std::string &name, const std::string &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:1170
z-axis acceleration (local/vehicle frame) (m/sec2)
#define CMT_OUTPUTSETTINGS_TIMESTAMP_SAMPLECNT
Definition: cmtdef.h:625
char BASE_IMPEXP * strcpy(char *dest, size_t destSize, const char *source) MRPT_NO_THROWS
An OS-independent version of strcpy.
Definition: os.cpp:296
#define CMT_OUTPUTMODE_CALIB
Definition: cmtdef.h:614
A class for interfacing XSens 3rd generation Inertial Measuring Units (IMUs), the "XSens MTi" model...
Definition: CIMUXSens.h:48
virtual ~CIMUXSens()
Destructor.
Definition: CIMUXSens.cpp:81
mrpt::system::TTimeStamp m_timeStartTT
Definition: CIMUXSens.h:59
This class allows loading and storing values and vectors of different types from a configuration text...
A structure containing MT data + timestamp and formatting information.
Definition: cmtpacket.h:24
int read_int(const std::string &section, const std::string &name, int defaultValue, bool failIfNotFound=false) const
pitch angular velocity (local/vehicle frame) (rad/sec)
void BASE_IMPEXP sleep(int time_ms) MRPT_NO_THROWS
An OS-independent method for sending the current thread to "sleep" for a given period of time...
Definition: threads.cpp:57
XsensResultValue
Xsens return values.
Definition: xsens_std.h:30
void initialize()
Turns on the xSens device and configure it for getting orientation data.
Definition: CIMUXSens.cpp:287
This namespace contains representation of robot actions and observations.
double m_roll
The roll (rotation around x-axis / back-front-line)
Definition: cmtdef.h:1169
uint32_t m_baudrate
The baudrate at which an Xsens device was detected.
Definition: cmtdef.h:1098
#define DEG2RAD
GLsizei const GLchar ** string
Definition: glext.h:3919
#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
void loadConfig_sensorSpecific(const mrpt::utils::CConfigFileBase &configSource, const std::string &iniSection)
See the class documentation at the top for expected parameters.
Definition: CIMUXSens.cpp:363
mrpt::poses::CPose3D m_sensorPose
Definition: CIMUXSens.h:61
double m_data[3]
Definition: cmtdef.h:1160
int m_COMbauds
This serial port will be attempted to be opened automatically when this class is first used to reques...
Definition: CIMUXSens.h:56
Dynamic list class.
Definition: xsens_list.h:60
GLint mode
Definition: glext.h:5078
unsigned __int64 uint64_t
Definition: rptypes.h:52
#define cmt3
Definition: CIMUXSens.cpp:40
unsigned int m_toutCounter
Timeout counter (for internal use only)
Definition: CIMUXSens.h:71
uint32_t length(void) const
Returns the number of items currently in the list.
Definition: xsens_list.h:143
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:266
uint32_t CmtOutputMode
An output mode bit-field.
Definition: cmtdef.h:895
#define ASSERT_(f)
void appendObservation(const mrpt::utils::CSerializablePtr &obj)
Like appendObservations() but for just one observation.
double m_yaw
The yaw (rotation around z-axis / down-up-line)
Definition: cmtdef.h:1171
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:122
GLuint res
Definition: glext.h:6298
orientation roll absolute value (global/navigation frame) (rad)
yaw angular velocity (local/vehicle frame) (rad/sec)
A structure for storing device modes.
Definition: cmtdef.h:978
char m_portName[32]
The port name.
Definition: cmtdef.h:1101
roll angular velocity (local/vehicle frame) (rad/sec)
unsigned __int32 uint32_t
Definition: rptypes.h:49
#define CMT_OUTPUTMODE_ORIENT
Definition: cmtdef.h:615
#define deviceId
Definition: CIMUXSens.cpp:41
x-axis acceleration (local/vehicle frame) (m/sec2)
uint32_t CmtDeviceId
The type of a Device Id.
Definition: cmtdef.h:851
#define CMT_OUTPUTSETTINGS_CALIBMODE_ACCGYR
Definition: cmtdef.h:631
uint64_t CmtOutputSettings
An output settings bit-field.
Definition: cmtdef.h:897



Page generated by Doxygen 1.8.14 for MRPT 1.5.9 Git: 690a4699f Wed Apr 15 19:29:53 2020 +0200 at miƩ abr 15 19:30:12 CEST 2020