MRPT  1.9.9
CSickLaserUSB.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 #include <mrpt/system/crc.h>
15 #include <mrpt/system/os.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() : m_serialNumber("LASER001")
34 {
36  m_sensorLabel = "SICKLMS";
38  MRPT_END
39 }
40 
41 /*-------------------------------------------------------------
42  ~CSickLaserUSB
43 -------------------------------------------------------------*/
45 {
46  delete m_usbConnection;
47  m_usbConnection = nullptr;
48 }
49 
50 /*-------------------------------------------------------------
51  doProcessSimple
52 -------------------------------------------------------------*/
54  bool& outThereIsObservation,
55  mrpt::obs::CObservation2DRangeScan& outObservation, bool& hardwareError)
56 {
57  outThereIsObservation = false;
58  hardwareError = false;
59 
61  {
62  hardwareError = true;
63  return;
64  }
65 
66  vector<float> ranges;
67  unsigned char LMS_stat;
68  uint32_t board_timestamp;
69  bool is_mm_mode;
70 
72 
73  // Wait for a scan:
75  ranges, LMS_stat, board_timestamp, is_mm_mode))
76  return;
77 
78  // Yes, we have a new scan:
79 
80  // -----------------------------------------------
81  // Extract the observation:
82  // -----------------------------------------------
83  uint32_t AtUI = 0;
84  if (m_timeStartUI == 0)
85  {
86  m_timeStartUI = board_timestamp;
88  }
89  else
90  AtUI = board_timestamp - m_timeStartUI;
91 
92  /* Board time is ms, -50ms aprox delay for scan sending from the scanner*/
93  auto AtDO = std::chrono::milliseconds(AtUI - 50);
94  outObservation.timestamp = m_timeStartTT + AtDO;
95 
96  outObservation.sensorLabel = m_sensorLabel; // Set label
97 
98  // Extract the timestamp of the sensor:
99 
100  // And the scan ranges:
101  outObservation.rightToLeft = true;
102  outObservation.aperture = M_PIf;
103  outObservation.maxRange = is_mm_mode ? 32.7 : 81.0;
104  outObservation.stdError = 0.003f;
105  outObservation.sensorPose = m_sensorPose;
106 
107  outObservation.resizeScan(ranges.size());
108 
109  for (size_t i = 0; i < ranges.size(); i++)
110  {
111  outObservation.setScanRange(i, ranges[i]);
112  outObservation.setScanRangeValidity(
113  i, (ranges[i] <= outObservation.maxRange));
114  }
115 
116  // Do filter:
119  // Do show preview:
121 
122  outThereIsObservation = true;
123 }
124 
125 /*-------------------------------------------------------------
126  loadConfig_sensorSpecific
127 -------------------------------------------------------------*/
129  const mrpt::config::CConfigFileBase& configSource,
130  const std::string& iniSection)
131 {
132  m_serialNumber = configSource.read_string(
133  iniSection, "SICKUSB_serialNumber", m_serialNumber);
135  configSource.read_float(iniSection, "pose_x", 0),
136  configSource.read_float(iniSection, "pose_y", 0),
137  configSource.read_float(iniSection, "pose_z", 0),
138  DEG2RAD(configSource.read_float(iniSection, "pose_yaw", 0)),
139  DEG2RAD(configSource.read_float(iniSection, "pose_pitch", 0)),
140  DEG2RAD(configSource.read_float(iniSection, "pose_roll", 0)));
141  // Parent options:
142  C2DRangeFinderAbstract::loadCommonParams(configSource, iniSection);
143 }
144 
145 /*-------------------------------------------------------------
146  turnOn
147 -------------------------------------------------------------*/
148 bool CSickLaserUSB::turnOn() { return true; }
149 /*-------------------------------------------------------------
150  turnOff
151 -------------------------------------------------------------*/
152 bool CSickLaserUSB::turnOff() { return true; }
153 /*-------------------------------------------------------------
154  checkControllerIsConnected
155 -------------------------------------------------------------*/
157 {
158  // If device is already open, thats ok:
159  if (m_usbConnection->isOpen()) return true;
160 
161  // If it isn't, try to open it now:
162  try
163  {
166  std::this_thread::sleep_for(10ms);
167  m_usbConnection->SetTimeouts(10, 20); // read, write, in milliseconds
168  std::this_thread::sleep_for(10ms);
169  m_usbConnection->SetLatencyTimer(1); // 1ms, the minimum
170  std::this_thread::sleep_for(10ms);
171 
173  "[CSickLaserUSB] USB DEVICE S/N:'%s' OPEN SUCCESSFULLY!!!\n",
174  m_serialNumber.c_str());
175  return true;
176  }
177  catch (const std::exception& e)
178  {
180  "[CSickLaserUSB] ERROR TRYING TO OPEN USB DEVICE S/N:'%s'\n%s",
181  m_serialNumber.c_str(), e.what());
182  return false;
183  }
184 }
185 
186 /*-------------------------------------------------------------
187  waitContinuousSampleFrame
188 -------------------------------------------------------------*/
190  vector<float>& out_ranges_meters, unsigned char& LMS_status,
191  uint32_t& out_board_timestamp, bool& is_mm_mode)
192 {
193  size_t nRead, nBytesToRead;
194  size_t nFrameBytes = 0;
195  size_t lenghtField;
196  unsigned char buf[2000];
197  buf[2] = buf[3] = 0;
198 
199  while (nFrameBytes < (lenghtField = (6 + (buf[2] | (buf[3] << 8)))) +
200  5 /* for 32bit timestamp + end-flag */)
201  {
202  if (lenghtField > 800)
203  {
204  cout << "#";
205  nFrameBytes = 0; // No es cabecera de trama correcta
206  buf[2] = buf[3] = 0;
207  }
208 
209  if (nFrameBytes < 4)
210  nBytesToRead = 1;
211  else
212  nBytesToRead =
213  (5 /* for 32bit timestamp + end-flag */ + lenghtField) -
214  nFrameBytes;
215 
216  try
217  {
218  nRead = m_usbConnection->ReadSync(buf + nFrameBytes, nBytesToRead);
219  }
220  catch (const std::exception& e)
221  {
222  // Disconnected?
224  "[CSickLaserUSB::waitContinuousSampleFrame] Disconnecting due "
225  "to comms error: %s\n",
226  e.what());
228  m_timeStartUI = 0;
229  return false;
230  }
231 
232  if (nRead == 0 && nFrameBytes == 0) return false;
233 
234  if (nRead > 0)
235  {
236  // Lectura OK:
237  // Era la primera?
238  if (nFrameBytes > 1 || (!nFrameBytes && buf[0] == 0x02) ||
239  (nFrameBytes == 1 && buf[1] == 0x80))
240  nFrameBytes += nRead;
241  else
242  {
243  nFrameBytes = 0; // No es cabecera de trama correcta
244  buf[2] = buf[3] = 0;
245  }
246  }
247  }
248 
249  // Frame received
250  // --------------------------------------------------------------------------
251  // | STX | ADDR | L1 | L2 | COM | INF1 | INF2 | DATA | STA | CRC1 | CRC2
252  // |
253  // --------------------------------------------------------------------------
254 
255  // Trama completa:
256  // Checkear que el byte de comando es 0xB0:
257  if (buf[4] != 0xB0) return false;
258 
259  // GET FRAME INFO
260  int info = buf[5] | (buf[6] << 8); // Little Endian
261  int n_points = info & 0x01FF;
262  is_mm_mode = 0 != ((info & 0xC000) >> 14); // 0x00: cm 0x01: mm
263 
264  out_ranges_meters.resize(n_points);
265 
266  // Copiar rangos:
267  short mask = is_mm_mode ? 0x7FFF : 0x1FFF;
268  float meters_scale = is_mm_mode ? 0.001f : 0.01f;
269 
270  for (int i = 0; i < n_points; i++)
271  out_ranges_meters[i] =
272  ((buf[7 + i * 2] | (buf[8 + i * 2] << 8)) & mask) * meters_scale;
273 
274  // Status
275  LMS_status = buf[lenghtField - 3];
276 
277  // End frame:
278  if (buf[nFrameBytes - 1] != 0x55)
279  {
280 // cerr << format("[CSickLaserUSB::waitContinuousSampleFrame] bad end flag") <<
281 // endl;
282 #ifdef _WIN32
283  OutputDebugStringA(
284  "[CSickLaserUSB::waitContinuousSampleFrame] bad end flag\n");
285 #endif
286  return false; // Bad CRC
287  }
288 
289  // CRC:
290  const uint16_t CRC = mrpt::system::compute_CRC16(buf, lenghtField - 2);
291  const uint16_t CRC_packet =
292  buf[lenghtField - 2] | (buf[lenghtField - 1] << 8);
293  if (CRC_packet != CRC)
294  {
295  const string s = format(
296  "[CSickLaserUSB::waitContinuousSampleFrame] bad CRC len=%u "
297  "nptns=%u: %i != %i\n",
298  unsigned(lenghtField), unsigned(n_points), CRC_packet, CRC);
299  cerr << s;
300 #ifdef _WIN32
301  OutputDebugStringA(s.c_str());
302 #endif
303  return false; // Bad CRC
304  }
305 
306  // Get USB board timestamp:
307  out_board_timestamp = (uint32_t(buf[nFrameBytes - 5]) << 24) |
308  (uint32_t(buf[nFrameBytes - 4]) << 16) |
309  (uint32_t(buf[nFrameBytes - 3]) << 8) |
310  uint32_t(buf[nFrameBytes - 2]);
311 
312  // All OK
313  return true;
314 }
GLenum GLint GLuint mask
Definition: glext.h:4062
uint32_t m_timeStartUI
Time of the first data packet, for synchronization purposes.
Definition: CSickLaserUSB.h:71
bool isOpen()
Checks whether the chip has been successfully open.
#define MRPT_START
Definition: exceptions.h:241
unsigned __int16 uint16_t
Definition: rptypes.h:47
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.
void loadConfig_sensorSpecific(const mrpt::config::CConfigFileBase &configSource, const std::string &iniSection) override
See the class documentation at the top for expected parameters.
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:62
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
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:3682
void filterByExclusionAngles(mrpt::obs::CObservation2DRangeScan &obs) const
Mark as invalid those ranges in a set of forbiden angle ranges.
void doProcessSimple(bool &outThereIsObservation, mrpt::obs::CObservation2DRangeScan &outObservation, bool &hardwareError) override
Specific laser scanner "software drivers" must process here new data from the I/O stream...
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:67
This namespace contains representation of robot actions and observations.
#define M_PIf
Definition: common.h:61
void OpenBySerialNumber(const std::string &serialNumber)
Open by device serial number.
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:
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...
bool turnOff() override
Disables the scanning mode (in this class this has no effect).
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:84
void resizeScan(const size_t len)
Resizes all data vectors to allocate a given number of scan rays.
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:16
#define MRPT_END
Definition: exceptions.h:245
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)...
bool turnOn() override
Enables the scanning mode (in this class this has no effect).
void processPreview(const mrpt::obs::CObservation2DRangeScan &obs)
Must be called inside the capture method to allow optional GUI preview of scans.
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:76
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:50
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,...)
~CSickLaserUSB() override
Destructor.
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:72
void setScanRangeValidity(const size_t i, const bool val)



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: c26d46ba6 Thu Jul 18 13:03:42 2019 +0200 at jue jul 18 13:10:17 CEST 2019