Main MRPT website > C++ reference for MRPT 1.5.7
CSickLaserUSB.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 #include <mrpt/utils/CStream.h>
13 #include <mrpt/utils/utils_defs.h>
14 #include <mrpt/system/os.h>
15 #include <mrpt/utils/crc.h>
17 
18 
19 #ifdef MRPT_OS_WINDOWS
20  #include <windows.h>
21 #endif
22 
24 
25 using namespace std;
26 using namespace mrpt;
27 using namespace mrpt::utils;
28 using namespace mrpt::obs;
29 using namespace mrpt::hwdrivers;
30 using namespace mrpt::poses;
31 
32 
33 /*-------------------------------------------------------------
34  CSickLaserUSB
35 -------------------------------------------------------------*/
36 CSickLaserUSB::CSickLaserUSB() :
37  m_usbConnection( NULL ),
38  m_serialNumber ( "LASER001" ),
39  m_timeStartUI ( 0 )
40 {
42  m_sensorLabel = "SICKLMS";
44  MRPT_END
45 }
46 
47 /*-------------------------------------------------------------
48  ~CSickLaserUSB
49 -------------------------------------------------------------*/
51 {
52  delete m_usbConnection;
53  m_usbConnection = NULL;
54 }
55 
56 /*-------------------------------------------------------------
57  doProcessSimple
58 -------------------------------------------------------------*/
60  bool &outThereIsObservation,
61  mrpt::obs::CObservation2DRangeScan &outObservation,
62  bool &hardwareError )
63 {
64  outThereIsObservation = false;
65  hardwareError = false;
66 
68  {
69  hardwareError = true;
70  return;
71  }
72 
73  vector<float> ranges;
74  unsigned char LMS_stat;
75  uint32_t board_timestamp;
76  bool is_mm_mode;
77 
79 
80  // Wait for a scan:
81  if (!waitContinuousSampleFrame( ranges,LMS_stat, board_timestamp, is_mm_mode ))
82  return;
83 
84  // Yes, we have a new scan:
85 
86  // -----------------------------------------------
87  // Extract the observation:
88  // -----------------------------------------------
89  uint32_t AtUI = 0;
90  if( m_timeStartUI == 0 )
91  {
92  m_timeStartUI = board_timestamp;
94  }
95  else AtUI = board_timestamp - m_timeStartUI;
96 
97  mrpt::system::TTimeStamp AtDO = mrpt::system::secondsToTimestamp( AtUI * 1e-3 /* Board time is ms */ );
98  outObservation.timestamp = m_timeStartTT + AtDO - mrpt::system::secondsToTimestamp( 0.05 ); // 50 ms delay for scan sending from the scanner
99 
100  outObservation.sensorLabel = m_sensorLabel; // Set label
101 
102  // Extract the timestamp of the sensor:
103 
104  // And the scan ranges:
105  outObservation.rightToLeft = true;
106  outObservation.aperture = M_PIf;
107  outObservation.maxRange = is_mm_mode ? 32.7 : 81.0;
108  outObservation.stdError = 0.003f;
109  outObservation.sensorPose = m_sensorPose;
110 
111  outObservation.resizeScan(ranges.size());
112 
113  for (size_t i=0;i<ranges.size();i++) {
114  outObservation.setScanRange(i, ranges[i]);
115  outObservation.setScanRangeValidity(i, (ranges[i] <= outObservation.maxRange) );
116  }
117 
118  // Do filter:
121  // Do show preview:
123 
124  outThereIsObservation = true;
125 }
126 
127 /*-------------------------------------------------------------
128  loadConfig_sensorSpecific
129 -------------------------------------------------------------*/
131  const mrpt::utils::CConfigFileBase &configSource,
132  const std::string &iniSection )
133 {
134  m_serialNumber = configSource.read_string(iniSection,"SICKUSB_serialNumber",m_serialNumber);
136  configSource.read_float(iniSection,"pose_x",0),
137  configSource.read_float(iniSection,"pose_y",0),
138  configSource.read_float(iniSection,"pose_z",0),
139  DEG2RAD( configSource.read_float(iniSection,"pose_yaw",0) ),
140  DEG2RAD( configSource.read_float(iniSection,"pose_pitch",0) ),
141  DEG2RAD( configSource.read_float(iniSection,"pose_roll",0) )
142  );
143  // Parent options:
144  C2DRangeFinderAbstract::loadCommonParams(configSource, iniSection);
145 }
146 
147 /*-------------------------------------------------------------
148  turnOn
149 -------------------------------------------------------------*/
151 {
152  return true;
153 }
154 
155 /*-------------------------------------------------------------
156  turnOff
157 -------------------------------------------------------------*/
159 {
160  return true;
161 }
162 
163 /*-------------------------------------------------------------
164  checkControllerIsConnected
165 -------------------------------------------------------------*/
167 {
168  // If device is already open, thats ok:
169  if (m_usbConnection->isOpen())
170  return true;
171 
172  // If it isn't, try to open it now:
173  try
174  {
178  m_usbConnection->SetTimeouts( 10, 20 ); // read, write, in milliseconds
180  m_usbConnection->SetLatencyTimer(1); // 1ms, the minimum
182 
183  MRPT_LOG_INFO_FMT("[CSickLaserUSB] USB DEVICE S/N:'%s' OPEN SUCCESSFULLY!!!\n",m_serialNumber.c_str() );
184  return true;
185  }
186  catch(std::exception &e)
187  {
188  MRPT_LOG_ERROR_FMT("[CSickLaserUSB] ERROR TRYING TO OPEN USB DEVICE S/N:'%s'\n%s",m_serialNumber.c_str(),e.what() );
189  return false;
190  }
191 }
192 
193 /*-------------------------------------------------------------
194  waitContinuousSampleFrame
195 -------------------------------------------------------------*/
197  vector<float> &out_ranges_meters,
198  unsigned char &LMS_status,
199  uint32_t &out_board_timestamp,
200  bool &is_mm_mode )
201 {
202  size_t nRead,nBytesToRead;
203  size_t nFrameBytes = 0;
204  size_t lenghtField;
205  unsigned char buf[2000];
206  buf[2]=buf[3]=0;
207 
208  while ( nFrameBytes < (lenghtField=( 6 + (buf[2] | (buf[3] << 8))) ) + 5 /* for 32bit timestamp + end-flag */ )
209  {
210  if (lenghtField>800)
211  {
212  cout << "#";
213  nFrameBytes = 0; // No es cabecera de trama correcta
214  buf[2]=buf[3]=0;
215  }
216 
217  if (nFrameBytes<4)
218  nBytesToRead = 1;
219  else
220  nBytesToRead = (5 /* for 32bit timestamp + end-flag */ + lenghtField) - nFrameBytes;
221 
222  try
223  {
224  nRead = m_usbConnection->ReadSync( buf+nFrameBytes,nBytesToRead );
225  }
226  catch (std::exception &e)
227  {
228  // Disconnected?
229  MRPT_LOG_ERROR_FMT("[CSickLaserUSB::waitContinuousSampleFrame] Disconnecting due to comms error: %s\n", e.what());
231  m_timeStartUI = 0;
232  return false;
233  }
234 
235  if ( nRead == 0 && nFrameBytes==0 )
236  return false;
237 
238  if (nRead>0)
239  {
240  // Lectura OK:
241  // Era la primera?
242  if (nFrameBytes>1 || (!nFrameBytes && buf[0]==0x02) || (nFrameBytes==1 && buf[1]==0x80))
243  nFrameBytes+=nRead;
244  else
245  {
246  nFrameBytes = 0; // No es cabecera de trama correcta
247  buf[2]=buf[3]=0;
248  }
249  }
250  }
251 
252  // Frame received
253  // --------------------------------------------------------------------------
254  // | STX | ADDR | L1 | L2 | COM | INF1 | INF2 | DATA | STA | CRC1 | CRC2 |
255  // --------------------------------------------------------------------------
256 
257  // Trama completa:
258  // Checkear que el byte de comando es 0xB0:
259  if ( buf[4]!=0xB0 ) return false;
260 
261  // GET FRAME INFO
262  int info = buf[5] | (buf[6] << 8); // Little Endian
263  int n_points = info & 0x01FF;
264  is_mm_mode = 0 != ((info & 0xC000) >> 14); // 0x00: cm 0x01: mm
265 
266  out_ranges_meters.resize(n_points);
267 
268  // Copiar rangos:
269  short mask = is_mm_mode ? 0x7FFF : 0x1FFF;
270  float meters_scale = is_mm_mode ? 0.001f : 0.01f;
271 
272  for (int i=0;i<n_points;i++)
273  out_ranges_meters[i] = ( (buf[7+i*2] | (buf[8+i*2] << 8)) & mask ) * meters_scale;
274 
275  // Status
276  LMS_status = buf[lenghtField-3];
277 
278  // End frame:
279  if (buf[nFrameBytes-1]!=0x55)
280  {
281  //cerr << format("[CSickLaserUSB::waitContinuousSampleFrame] bad end flag") << endl;
282 #ifdef MRPT_OS_WINDOWS
283  OutputDebugStringA("[CSickLaserUSB::waitContinuousSampleFrame] bad end flag\n");
284 #endif
285  return false; // Bad CRC
286  }
287 
288  // CRC:
289  const uint16_t CRC = mrpt::utils::compute_CRC16(buf,lenghtField-2);
290  const uint16_t CRC_packet = buf[lenghtField-2] | ( buf[lenghtField-1] << 8);
291  if (CRC_packet!=CRC)
292  {
293  const string s = format("[CSickLaserUSB::waitContinuousSampleFrame] bad CRC len=%u nptns=%u: %i != %i\n", unsigned(lenghtField),unsigned(n_points), CRC_packet, CRC);
294  cerr << s;
295 #ifdef MRPT_OS_WINDOWS
296  OutputDebugStringA(s.c_str());
297 #endif
298  return false; // Bad CRC
299  }
300 
301  // Get USB board timestamp:
302  out_board_timestamp =
303  (uint32_t(buf[nFrameBytes-5]) << 24) |
304  (uint32_t(buf[nFrameBytes-4]) << 16) |
305  (uint32_t(buf[nFrameBytes-3]) << 8) |
306  uint32_t(buf[nFrameBytes-2]);
307 
308  // All OK
309  return true;
310 }
void SetTimeouts(unsigned long dwReadTimeout_ms, unsigned long dwWriteTimeout_ms)
Change read & write timeouts, in milliseconds.
uint64_t TTimeStamp
A system independent time type, it holds the the number of 100-nanosecond intervals since January 1...
Definition: datetime.h:30
void doProcessSimple(bool &outThereIsObservation, mrpt::obs::CObservation2DRangeScan &outObservation, bool &hardwareError)
Specific laser scanner "software drivers" must process here new data from the I/O stream...
virtual ~CSickLaserUSB()
Destructor.
GLenum GLint GLuint mask
Definition: glext.h:3888
uint32_t m_timeStartUI
Time of the first data packet, for synchronization purposes.
Definition: CSickLaserUSB.h:65
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: zip.h:16
unsigned __int16 uint16_t
Definition: rptypes.h:46
float read_float(const std::string &section, const std::string &name, float defaultValue, bool failIfNotFound=false) const
void setScanRange(const size_t i, const float val)
std::string m_sensorLabel
See CGenericSensor.
This "software driver" implements the communication protocol for interfacing a SICK LMS2XX laser scan...
Definition: CSickLaserUSB.h:57
void OpenBySerialNumber(const std::string &serialNumber)
Open by device serial number.
bool isOpen()
Checks whether the chip has been successfully open.
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
STL namespace.
void filterByExclusionAreas(mrpt::obs::CObservation2DRangeScan &obs) const
Mark as invalid those points which (x,y) coordinates fall within the exclusion polygons.
float stdError
The "sigma" error of the device in meters, used while inserting the scan in an occupancy grid...
#define MRPT_LOG_ERROR_FMT(_FMT_STRING,...)
GLdouble s
Definition: glext.h:3602
bool turnOn()
Enables the scanning mode (in this class this has no effect).
void filterByExclusionAngles(mrpt::obs::CObservation2DRangeScan &obs) const
Mark as invalid those ranges in a set of forbiden angle ranges.
This class allows loading and storing values and vectors of different types from a configuration text...
float maxRange
The maximum range allowed by the device, in meters (e.g. 80m, 50m,...)
size_t ReadSync(void *Buffer, size_t Count)
Tries to read, raising no exception if not all the bytes are available, but raising one if there is s...
#define M_PIf
#define MRPT_END
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
This namespace contains representation of robot actions and observations.
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:21
uint16_t BASE_IMPEXP compute_CRC16(const std::vector< uint8_t > &data, const uint16_t gen_pol=0x8005)
Computes the CRC16 checksum of a block of data.
Definition: crc.cpp:22
#define DEG2RAD
void loadCommonParams(const mrpt::utils::CConfigFileBase &configSource, const std::string &iniSection)
Should be call by derived classes at "loadConfig" (loads exclusion areas AND exclusion angles)...
GLsizei const GLchar ** string
Definition: glext.h:3919
CInterfaceFTDI * m_usbConnection
Definition: CSickLaserUSB.h:62
#define IMPLEMENTS_GENERIC_SENSOR(class_name, NameSpace)
This must be inserted in all CGenericSensor classes implementation files:
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CPoint.h:17
#define MRPT_START
std::string sensorLabel
An arbitrary label that can be used to identify the sensor.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
mrpt::system::TTimeStamp timestamp
The associated UTC time-stamp. Where available, this should contain the accurate satellite-based time...
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
void SetLatencyTimer(unsigned char latency_ms)
Change the latency timer (in milliseconds) implemented on the FTDI chip: for a few ms...
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
void resizeScan(const size_t len)
Resizes all data vectors to allocate a given number of scan rays.
backing_store_ptr info
Definition: jmemsys.h:170
void processPreview(const mrpt::obs::CObservation2DRangeScan &obs)
Must be called inside the capture method to allow optional GUI preview of scans.
bool turnOff()
Disables the scanning mode (in this class this has no effect).
#define MRPT_LOG_INFO_FMT(_FMT_STRING,...)
float aperture
The "aperture" or field-of-view of the range finder, in radians (typically M_PI = 180 degrees)...
poses::CPose3D m_sensorPose
The sensor 6D pose:
Definition: CSickLaserUSB.h:70
mrpt::system::TTimeStamp BASE_IMPEXP secondsToTimestamp(const double nSeconds)
Transform a time interval (in seconds) into TTimeStamp (e.g.
Definition: datetime.cpp:219
void loadConfig_sensorSpecific(const mrpt::utils::CConfigFileBase &configSource, const std::string &iniSection)
See the class documentation at the top for expected parameters.
bool waitContinuousSampleFrame(std::vector< float > &ranges, unsigned char &LMS_status, uint32_t &out_board_timestamp, bool &is_mm_mode)
unsigned __int32 uint32_t
Definition: rptypes.h:49
A definition of a CStream actually representing a USB connection to a FTDI chip.
void ResetDevice()
Reset the USB device.
mrpt::poses::CPose3D sensorPose
The 6D pose of the sensor on the robot at the moment of starting the scan.
bool rightToLeft
The scanning direction: true=counterclockwise; false=clockwise.
mrpt::system::TTimeStamp m_timeStartTT
Definition: CSickLaserUSB.h:66
void setScanRangeValidity(const size_t i, const bool val)
void Close()
Close the USB device.



Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019