Main MRPT website > C++ reference for MRPT 1.5.6
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 -------------------------------------------------------------*/
37  m_usbConnection( NULL ),
38  m_serialNumber ( "LASER001" ),
39  m_timeStartUI ( 0 )
40 {
42  m_sensorLabel = "SICKLMS";
43  m_usbConnection = new CInterfaceFTDI();
44  MRPT_END
45 }
46 
47 /*-------------------------------------------------------------
48  ~CSickLaserUSB
49 -------------------------------------------------------------*/
50 CSickLaserUSB::~CSickLaserUSB()
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...
uint32_t m_timeStartUI
Time of the first data packet, for synchronization purposes.
Definition: CSickLaserUSB.h:65
float read_float(const std::string &section, const std::string &name, float defaultValue, bool failIfNotFound=false) const
unsigned __int16 uint16_t
Definition: rptypes.h:46
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
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,...)
bool turnOn()
Enables the scanning mode (in this class this has no effect).
void filterByExclusionAreas(mrpt::obs::CObservation2DRangeScan &obs) const
Mark as invalid those points which (x,y) coordinates fall within the exclusion polygons.
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,...)
std::string read_string(const std::string &section, const std::string &name, const std::string &defaultValue, bool failIfNotFound=false) const
GLdouble s
Definition: glew.h:1295
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
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)...
CInterfaceFTDI * m_usbConnection
Definition: CSickLaserUSB.h:62
#define IMPLEMENTS_GENERIC_SENSOR(class_name, NameSpace)
This must be inserted in all CGenericSensor classes implementation files:
#define MRPT_START
std::string sensorLabel
An arbitrary label that can be used to identify the sensor.
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...
GLsizei const GLcharARB ** string
Definition: glew.h:3293
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,...)
void filterByExclusionAngles(mrpt::obs::CObservation2DRangeScan &obs) const
Mark as invalid those ranges in a set of forbiden angle ranges.
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.
GLenum GLint GLuint mask
Definition: glew.h:1752
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.6 for MRPT 1.5.6 Git: 4c65e84 Tue Apr 24 08:18:17 2018 +0200 at mar abr 24 08:26:17 CEST 2018