Main MRPT website > C++ reference for MRPT 1.9.9
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-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 
13 #include <mrpt/system/os.h>
14 #include <mrpt/system/crc.h>
16 
17 #ifdef _WIN32
18 #include <windows.h>
19 #endif
20 
22 
23 using namespace std;
24 using namespace mrpt;
25 using namespace mrpt::comms;
26 using namespace mrpt::obs;
27 using namespace mrpt::hwdrivers;
28 using namespace mrpt::poses;
29 
30 /*-------------------------------------------------------------
31  CSickLaserUSB
32 -------------------------------------------------------------*/
33 CSickLaserUSB::CSickLaserUSB()
34  : m_usbConnection(nullptr), m_serialNumber("LASER001"), m_timeStartUI(0)
35 {
37  m_sensorLabel = "SICKLMS";
39  MRPT_END
40 }
41 
42 /*-------------------------------------------------------------
43  ~CSickLaserUSB
44 -------------------------------------------------------------*/
46 {
47  delete m_usbConnection;
48  m_usbConnection = nullptr;
49 }
50 
51 /*-------------------------------------------------------------
52  doProcessSimple
53 -------------------------------------------------------------*/
55  bool& outThereIsObservation,
56  mrpt::obs::CObservation2DRangeScan& outObservation, bool& hardwareError)
57 {
58  outThereIsObservation = false;
59  hardwareError = false;
60 
62  {
63  hardwareError = true;
64  return;
65  }
66 
67  vector<float> ranges;
68  unsigned char LMS_stat;
69  uint32_t board_timestamp;
70  bool is_mm_mode;
71 
73 
74  // Wait for a scan:
76  ranges, LMS_stat, board_timestamp, is_mm_mode))
77  return;
78 
79  // Yes, we have a new scan:
80 
81  // -----------------------------------------------
82  // Extract the observation:
83  // -----------------------------------------------
84  uint32_t AtUI = 0;
85  if (m_timeStartUI == 0)
86  {
87  m_timeStartUI = board_timestamp;
89  }
90  else
91  AtUI = board_timestamp - m_timeStartUI;
92 
94  mrpt::system::secondsToTimestamp(AtUI * 1e-3 /* Board time is ms */);
95  outObservation.timestamp =
96  m_timeStartTT + AtDO -
98  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  {
115  outObservation.setScanRange(i, ranges[i]);
116  outObservation.setScanRangeValidity(
117  i, (ranges[i] <= outObservation.maxRange));
118  }
119 
120  // Do filter:
123  // Do show preview:
125 
126  outThereIsObservation = true;
127 }
128 
129 /*-------------------------------------------------------------
130  loadConfig_sensorSpecific
131 -------------------------------------------------------------*/
133  const mrpt::config::CConfigFileBase& configSource,
134  const std::string& iniSection)
135 {
136  m_serialNumber = configSource.read_string(
137  iniSection, "SICKUSB_serialNumber", m_serialNumber);
139  configSource.read_float(iniSection, "pose_x", 0),
140  configSource.read_float(iniSection, "pose_y", 0),
141  configSource.read_float(iniSection, "pose_z", 0),
142  DEG2RAD(configSource.read_float(iniSection, "pose_yaw", 0)),
143  DEG2RAD(configSource.read_float(iniSection, "pose_pitch", 0)),
144  DEG2RAD(configSource.read_float(iniSection, "pose_roll", 0)));
145  // Parent options:
146  C2DRangeFinderAbstract::loadCommonParams(configSource, iniSection);
147 }
148 
149 /*-------------------------------------------------------------
150  turnOn
151 -------------------------------------------------------------*/
152 bool CSickLaserUSB::turnOn() { return true; }
153 /*-------------------------------------------------------------
154  turnOff
155 -------------------------------------------------------------*/
156 bool CSickLaserUSB::turnOff() { return true; }
157 /*-------------------------------------------------------------
158  checkControllerIsConnected
159 -------------------------------------------------------------*/
161 {
162  // If device is already open, thats ok:
163  if (m_usbConnection->isOpen()) return true;
164 
165  // If it isn't, try to open it now:
166  try
167  {
170  std::this_thread::sleep_for(10ms);
171  m_usbConnection->SetTimeouts(10, 20); // read, write, in milliseconds
172  std::this_thread::sleep_for(10ms);
173  m_usbConnection->SetLatencyTimer(1); // 1ms, the minimum
174  std::this_thread::sleep_for(10ms);
175 
177  "[CSickLaserUSB] USB DEVICE S/N:'%s' OPEN SUCCESSFULLY!!!\n",
178  m_serialNumber.c_str());
179  return true;
180  }
181  catch (std::exception& e)
182  {
184  "[CSickLaserUSB] ERROR TRYING TO OPEN USB DEVICE S/N:'%s'\n%s",
185  m_serialNumber.c_str(), e.what());
186  return false;
187  }
188 }
189 
190 /*-------------------------------------------------------------
191  waitContinuousSampleFrame
192 -------------------------------------------------------------*/
194  vector<float>& out_ranges_meters, unsigned char& LMS_status,
195  uint32_t& out_board_timestamp, bool& is_mm_mode)
196 {
197  size_t nRead, nBytesToRead;
198  size_t nFrameBytes = 0;
199  size_t lenghtField;
200  unsigned char buf[2000];
201  buf[2] = buf[3] = 0;
202 
203  while (nFrameBytes < (lenghtField = (6 + (buf[2] | (buf[3] << 8)))) +
204  5 /* for 32bit timestamp + end-flag */)
205  {
206  if (lenghtField > 800)
207  {
208  cout << "#";
209  nFrameBytes = 0; // No es cabecera de trama correcta
210  buf[2] = buf[3] = 0;
211  }
212 
213  if (nFrameBytes < 4)
214  nBytesToRead = 1;
215  else
216  nBytesToRead =
217  (5 /* for 32bit timestamp + end-flag */ + lenghtField) -
218  nFrameBytes;
219 
220  try
221  {
222  nRead = m_usbConnection->ReadSync(buf + nFrameBytes, nBytesToRead);
223  }
224  catch (std::exception& e)
225  {
226  // Disconnected?
228  "[CSickLaserUSB::waitContinuousSampleFrame] Disconnecting due "
229  "to comms error: %s\n",
230  e.what());
232  m_timeStartUI = 0;
233  return false;
234  }
235 
236  if (nRead == 0 && nFrameBytes == 0) return false;
237 
238  if (nRead > 0)
239  {
240  // Lectura OK:
241  // Era la primera?
242  if (nFrameBytes > 1 || (!nFrameBytes && buf[0] == 0x02) ||
243  (nFrameBytes == 1 && buf[1] == 0x80))
244  nFrameBytes += nRead;
245  else
246  {
247  nFrameBytes = 0; // No es cabecera de trama correcta
248  buf[2] = buf[3] = 0;
249  }
250  }
251  }
252 
253  // Frame received
254  // --------------------------------------------------------------------------
255  // | STX | ADDR | L1 | L2 | COM | INF1 | INF2 | DATA | STA | CRC1 | CRC2
256  // |
257  // --------------------------------------------------------------------------
258 
259  // Trama completa:
260  // Checkear que el byte de comando es 0xB0:
261  if (buf[4] != 0xB0) return false;
262 
263  // GET FRAME INFO
264  int info = buf[5] | (buf[6] << 8); // Little Endian
265  int n_points = info & 0x01FF;
266  is_mm_mode = 0 != ((info & 0xC000) >> 14); // 0x00: cm 0x01: mm
267 
268  out_ranges_meters.resize(n_points);
269 
270  // Copiar rangos:
271  short mask = is_mm_mode ? 0x7FFF : 0x1FFF;
272  float meters_scale = is_mm_mode ? 0.001f : 0.01f;
273 
274  for (int i = 0; i < n_points; i++)
275  out_ranges_meters[i] =
276  ((buf[7 + i * 2] | (buf[8 + i * 2] << 8)) & mask) * meters_scale;
277 
278  // Status
279  LMS_status = buf[lenghtField - 3];
280 
281  // End frame:
282  if (buf[nFrameBytes - 1] != 0x55)
283  {
284 // cerr << format("[CSickLaserUSB::waitContinuousSampleFrame] bad end flag") <<
285 // endl;
286 #ifdef _WIN32
287  OutputDebugStringA(
288  "[CSickLaserUSB::waitContinuousSampleFrame] bad end flag\n");
289 #endif
290  return false; // Bad CRC
291  }
292 
293  // CRC:
294  const uint16_t CRC = mrpt::system::compute_CRC16(buf, lenghtField - 2);
295  const uint16_t CRC_packet =
296  buf[lenghtField - 2] | (buf[lenghtField - 1] << 8);
297  if (CRC_packet != CRC)
298  {
299  const string s = format(
300  "[CSickLaserUSB::waitContinuousSampleFrame] bad CRC len=%u "
301  "nptns=%u: %i != %i\n",
302  unsigned(lenghtField), unsigned(n_points), CRC_packet, CRC);
303  cerr << s;
304 #ifdef _WIN32
305  OutputDebugStringA(s.c_str());
306 #endif
307  return false; // Bad CRC
308  }
309 
310  // Get USB board timestamp:
311  out_board_timestamp = (uint32_t(buf[nFrameBytes - 5]) << 24) |
312  (uint32_t(buf[nFrameBytes - 4]) << 16) |
313  (uint32_t(buf[nFrameBytes - 3]) << 8) |
314  uint32_t(buf[nFrameBytes - 2]);
315 
316  // All OK
317  return true;
318 }
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:4050
uint32_t m_timeStartUI
Time of the first data packet, for synchronization purposes.
Definition: CSickLaserUSB.h:74
bool isOpen()
Checks whether the chip has been successfully open.
#define MRPT_START
Definition: exceptions.h:262
unsigned __int16 uint16_t
Definition: rptypes.h:44
std::string read_string(const std::string &section, const std::string &name, const std::string &defaultValue, bool failIfNotFound=false) const
#define MRPT_LOG_ERROR_FMT(_FMT_STRING,...)
void setScanRange(const size_t i, const float val)
std::string m_sensorLabel
See CGenericSensor.
double DEG2RAD(const double x)
Degrees to radians.
This "software driver" implements the communication protocol for interfacing a SICK LMS2XX laser scan...
Definition: CSickLaserUSB.h:65
mrpt::system::TTimeStamp now()
A shortcut for system::getCurrentTime.
Definition: datetime.h:75
Contains classes for various device interfaces.
float read_float(const std::string &section, const std::string &name, float 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...
GLdouble s
Definition: glext.h:3676
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.
float maxRange
The maximum range allowed by the device, in meters (e.g.
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...
This class allows loading and storing values and vectors of different types from a configuration text...
void Close()
Close the USB device.
void SetTimeouts(unsigned long dwReadTimeout_ms, unsigned long dwWriteTimeout_ms)
Change read & write timeouts, in milliseconds.
mrpt::comms::CInterfaceFTDI * m_usbConnection
Definition: CSickLaserUSB.h:70
This namespace contains representation of robot actions and observations.
#define M_PIf
Definition: common.h:61
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:16
void OpenBySerialNumber(const std::string &serialNumber)
Open by device serial number.
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:
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
void ResetDevice()
Reset the USB device.
std::string sensorLabel
An arbitrary label that can be used to identify the sensor.
Definition: CObservation.h:62
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
mrpt::system::TTimeStamp timestamp
The associated UTC time-stamp.
Definition: CObservation.h:60
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
A definition of a CStream actually representing a USB connection to a FTDI chip.
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:88
void resizeScan(const size_t len)
Resizes all data vectors to allocate a given number of scan rays.
#define MRPT_END
Definition: exceptions.h:266
void loadCommonParams(const mrpt::config::CConfigFileBase &configSource, const std::string &iniSection)
Should be call by derived classes at "loadConfig" (loads exclusion areas AND exclusion angles)...
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).
uint64_t TTimeStamp
A system independent time type, it holds the the number of 100-nanosecond intervals since January 1...
Definition: datetime.h:31
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:79
mrpt::system::TTimeStamp secondsToTimestamp(const double nSeconds)
Transform a time interval (in seconds) into TTimeStamp (e.g.
Definition: datetime.cpp:224
uint16_t 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:15
Serial and networking devices and utilities.
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:47
void loadConfig_sensorSpecific(const mrpt::config::CConfigFileBase &configSource, const std::string &iniSection)
See the class documentation at the top for expected parameters.
mrpt::poses::CPose3D sensorPose
The 6D pose of the sensor on the robot at the moment of starting the scan.
#define MRPT_LOG_INFO_FMT(_FMT_STRING,...)
void SetLatencyTimer(unsigned char latency_ms)
Change the latency timer (in milliseconds) implemented on the FTDI chip: for a few ms...
bool rightToLeft
The scanning direction: true=counterclockwise; false=clockwise.
mrpt::system::TTimeStamp m_timeStartTT
Definition: CSickLaserUSB.h:75
void setScanRangeValidity(const size_t i, const bool val)



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019