MRPT  2.0.4
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-2020, 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";
37  m_usbConnection = std::make_unique<CInterfaceFTDI>();
38  MRPT_END
39 }
40 
41 /*-------------------------------------------------------------
42  doProcessSimple
43 -------------------------------------------------------------*/
45  bool& outThereIsObservation,
46  mrpt::obs::CObservation2DRangeScan& outObservation, bool& hardwareError)
47 {
48  outThereIsObservation = false;
49  hardwareError = false;
50 
52  {
53  hardwareError = true;
54  return;
55  }
56 
57  vector<float> ranges;
58  unsigned char LMS_stat;
59  uint32_t board_timestamp;
60  bool is_mm_mode;
61 
63 
64  // Wait for a scan:
66  ranges, LMS_stat, board_timestamp, is_mm_mode))
67  return;
68 
69  // Yes, we have a new scan:
70 
71  // -----------------------------------------------
72  // Extract the observation:
73  // -----------------------------------------------
74  uint32_t AtUI = 0;
75  if (m_timeStartUI == 0)
76  {
77  m_timeStartUI = board_timestamp;
79  }
80  else
81  AtUI = board_timestamp - m_timeStartUI;
82 
83  /* Board time is ms, -50ms aprox delay for scan sending from the scanner*/
84  auto AtDO = std::chrono::milliseconds(AtUI - 50);
85  outObservation.timestamp = m_timeStartTT + AtDO;
86 
87  outObservation.sensorLabel = m_sensorLabel; // Set label
88 
89  // Extract the timestamp of the sensor:
90 
91  // And the scan ranges:
92  outObservation.rightToLeft = true;
93  outObservation.aperture = M_PIf;
94  outObservation.maxRange = is_mm_mode ? 32.7 : 81.0;
95  outObservation.stdError = 0.003f;
96  outObservation.sensorPose = m_sensorPose;
97 
98  outObservation.resizeScan(ranges.size());
99 
100  for (size_t i = 0; i < ranges.size(); i++)
101  {
102  outObservation.setScanRange(i, ranges[i]);
103  outObservation.setScanRangeValidity(
104  i, (ranges[i] <= outObservation.maxRange));
105  }
106 
107  // Do filter:
110  // Do show preview:
112 
113  outThereIsObservation = true;
114 }
115 
116 /*-------------------------------------------------------------
117  loadConfig_sensorSpecific
118 -------------------------------------------------------------*/
120  const mrpt::config::CConfigFileBase& configSource,
121  const std::string& iniSection)
122 {
123  m_serialNumber = configSource.read_string(
124  iniSection, "SICKUSB_serialNumber", m_serialNumber);
126  configSource.read_float(iniSection, "pose_x", 0),
127  configSource.read_float(iniSection, "pose_y", 0),
128  configSource.read_float(iniSection, "pose_z", 0),
129  DEG2RAD(configSource.read_float(iniSection, "pose_yaw", 0)),
130  DEG2RAD(configSource.read_float(iniSection, "pose_pitch", 0)),
131  DEG2RAD(configSource.read_float(iniSection, "pose_roll", 0)));
132  // Parent options:
133  C2DRangeFinderAbstract::loadCommonParams(configSource, iniSection);
134 }
135 
136 /*-------------------------------------------------------------
137  turnOn
138 -------------------------------------------------------------*/
139 bool CSickLaserUSB::turnOn() { return true; }
140 /*-------------------------------------------------------------
141  turnOff
142 -------------------------------------------------------------*/
143 bool CSickLaserUSB::turnOff() { return true; }
144 /*-------------------------------------------------------------
145  checkControllerIsConnected
146 -------------------------------------------------------------*/
148 {
149  // If device is already open, thats ok:
150  if (m_usbConnection->isOpen()) return true;
151 
152  // If it isn't, try to open it now:
153  try
154  {
155  m_usbConnection->OpenBySerialNumber(m_serialNumber);
156  m_usbConnection->ResetDevice();
157  std::this_thread::sleep_for(10ms);
158  m_usbConnection->SetTimeouts(10, 20); // read, write, in milliseconds
159  std::this_thread::sleep_for(10ms);
160  m_usbConnection->SetLatencyTimer(1); // 1ms, the minimum
161  std::this_thread::sleep_for(10ms);
162 
164  "[CSickLaserUSB] USB DEVICE S/N:'%s' OPEN SUCCESSFULLY!!!\n",
165  m_serialNumber.c_str());
166  return true;
167  }
168  catch (const std::exception& e)
169  {
171  "[CSickLaserUSB] ERROR TRYING TO OPEN USB DEVICE S/N:'%s'\n%s",
172  m_serialNumber.c_str(), e.what());
173  return false;
174  }
175 }
176 
177 /*-------------------------------------------------------------
178  waitContinuousSampleFrame
179 -------------------------------------------------------------*/
181  vector<float>& out_ranges_meters, unsigned char& LMS_status,
182  uint32_t& out_board_timestamp, bool& is_mm_mode)
183 {
184  size_t nRead, nBytesToRead;
185  size_t nFrameBytes = 0;
186  size_t lenghtField;
187  unsigned char buf[2000];
188  buf[2] = buf[3] = 0;
189 
190  while (nFrameBytes < (lenghtField = (6 + (buf[2] | (buf[3] << 8)))) +
191  5 /* for 32bit timestamp + end-flag */)
192  {
193  if (lenghtField > 800)
194  {
195  cout << "#";
196  nFrameBytes = 0; // No es cabecera de trama correcta
197  buf[2] = buf[3] = 0;
198  }
199 
200  if (nFrameBytes < 4)
201  nBytesToRead = 1;
202  else
203  nBytesToRead =
204  (5 /* for 32bit timestamp + end-flag */ + lenghtField) -
205  nFrameBytes;
206 
207  try
208  {
209  nRead = m_usbConnection->ReadSync(buf + nFrameBytes, nBytesToRead);
210  }
211  catch (const std::exception& e)
212  {
213  // Disconnected?
215  "[CSickLaserUSB::waitContinuousSampleFrame] Disconnecting due "
216  "to comms error: %s\n",
217  e.what());
218  m_usbConnection->Close();
219  m_timeStartUI = 0;
220  return false;
221  }
222 
223  if (nRead == 0 && nFrameBytes == 0) return false;
224 
225  if (nRead > 0)
226  {
227  // Lectura OK:
228  // Era la primera?
229  if (nFrameBytes > 1 || (!nFrameBytes && buf[0] == 0x02) ||
230  (nFrameBytes == 1 && buf[1] == 0x80))
231  nFrameBytes += nRead;
232  else
233  {
234  nFrameBytes = 0; // No es cabecera de trama correcta
235  buf[2] = buf[3] = 0;
236  }
237  }
238  }
239 
240  // Frame received
241  // --------------------------------------------------------------------------
242  // | STX | ADDR | L1 | L2 | COM | INF1 | INF2 | DATA | STA | CRC1 | CRC2
243  // |
244  // --------------------------------------------------------------------------
245 
246  // Trama completa:
247  // Checkear que el byte de comando es 0xB0:
248  if (buf[4] != 0xB0) return false;
249 
250  // GET FRAME INFO
251  int info = buf[5] | (buf[6] << 8); // Little Endian
252  int n_points = info & 0x01FF;
253  is_mm_mode = 0 != ((info & 0xC000) >> 14); // 0x00: cm 0x01: mm
254 
255  out_ranges_meters.resize(n_points);
256 
257  // Copiar rangos:
258  short mask = is_mm_mode ? 0x7FFF : 0x1FFF;
259  float meters_scale = is_mm_mode ? 0.001f : 0.01f;
260 
261  for (int i = 0; i < n_points; i++)
262  out_ranges_meters[i] =
263  ((buf[7 + i * 2] | (buf[8 + i * 2] << 8)) & mask) * meters_scale;
264 
265  // Status
266  LMS_status = buf[lenghtField - 3];
267 
268  // End frame:
269  if (buf[nFrameBytes - 1] != 0x55)
270  {
271 // cerr << format("[CSickLaserUSB::waitContinuousSampleFrame] bad end flag") <<
272 // endl;
273 #ifdef _WIN32
274  OutputDebugStringA(
275  "[CSickLaserUSB::waitContinuousSampleFrame] bad end flag\n");
276 #endif
277  return false; // Bad CRC
278  }
279 
280  // CRC:
281  const uint16_t CRC = mrpt::system::compute_CRC16(buf, lenghtField - 2);
282  const uint16_t CRC_packet =
283  buf[lenghtField - 2] | (buf[lenghtField - 1] << 8);
284  if (CRC_packet != CRC)
285  {
286  const string s = format(
287  "[CSickLaserUSB::waitContinuousSampleFrame] bad CRC len=%u "
288  "nptns=%u: %i != %i\n",
289  unsigned(lenghtField), unsigned(n_points), CRC_packet, CRC);
290  cerr << s;
291 #ifdef _WIN32
292  OutputDebugStringA(s.c_str());
293 #endif
294  return false; // Bad CRC
295  }
296 
297  // Get USB board timestamp:
298  out_board_timestamp = (uint32_t(buf[nFrameBytes - 5]) << 24) |
299  (uint32_t(buf[nFrameBytes - 4]) << 16) |
300  (uint32_t(buf[nFrameBytes - 3]) << 8) |
301  uint32_t(buf[nFrameBytes - 2]);
302 
303  // All OK
304  return true;
305 }
std::unique_ptr< mrpt::comms::CInterfaceFTDI > m_usbConnection
Definition: CSickLaserUSB.h:67
uint32_t m_timeStartUI
Time of the first data packet, for synchronization purposes.
Definition: CSickLaserUSB.h:71
#define MRPT_START
Definition: exceptions.h:241
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,...)
std::string std::string format(std::string_view fmt, ARGS &&... args)
Definition: format.h:26
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.
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...
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.
This class allows loading and storing values and vectors of different types from a configuration text...
constexpr double DEG2RAD(const double x)
Degrees to radians.
This namespace contains representation of robot actions and observations.
#define M_PIf
Definition: common.h:61
#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...
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 class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
void resizeScan(const size_t len)
Resizes all data vectors to allocate a given number of scan rays.
#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)
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,...)
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 2.0.4 Git: 33de1d0ad Sat Jun 20 11:02:42 2020 +0200 at sáb jun 20 17:35:17 CEST 2020