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 
93  /* Board time is ms, -50ms aprox delay for scan sending from the scanner*/
94  auto AtDO = std::chrono::milliseconds(AtUI - 50);
95  outObservation.timestamp = m_timeStartTT + AtDO;
96 
97  outObservation.sensorLabel = m_sensorLabel; // Set label
98 
99  // Extract the timestamp of the sensor:
100 
101  // And the scan ranges:
102  outObservation.rightToLeft = true;
103  outObservation.aperture = M_PIf;
104  outObservation.maxRange = is_mm_mode ? 32.7 : 81.0;
105  outObservation.stdError = 0.003f;
106  outObservation.sensorPose = m_sensorPose;
107 
108  outObservation.resizeScan(ranges.size());
109 
110  for (size_t i = 0; i < ranges.size(); i++)
111  {
112  outObservation.setScanRange(i, ranges[i]);
113  outObservation.setScanRangeValidity(
114  i, (ranges[i] <= outObservation.maxRange));
115  }
116 
117  // Do filter:
120  // Do show preview:
122 
123  outThereIsObservation = true;
124 }
125 
126 /*-------------------------------------------------------------
127  loadConfig_sensorSpecific
128 -------------------------------------------------------------*/
130  const mrpt::config::CConfigFileBase& configSource,
131  const std::string& iniSection)
132 {
133  m_serialNumber = configSource.read_string(
134  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  // Parent options:
143  C2DRangeFinderAbstract::loadCommonParams(configSource, iniSection);
144 }
145 
146 /*-------------------------------------------------------------
147  turnOn
148 -------------------------------------------------------------*/
149 bool CSickLaserUSB::turnOn() { return true; }
150 /*-------------------------------------------------------------
151  turnOff
152 -------------------------------------------------------------*/
153 bool CSickLaserUSB::turnOff() { return true; }
154 /*-------------------------------------------------------------
155  checkControllerIsConnected
156 -------------------------------------------------------------*/
158 {
159  // If device is already open, thats ok:
160  if (m_usbConnection->isOpen()) return true;
161 
162  // If it isn't, try to open it now:
163  try
164  {
167  std::this_thread::sleep_for(10ms);
168  m_usbConnection->SetTimeouts(10, 20); // read, write, in milliseconds
169  std::this_thread::sleep_for(10ms);
170  m_usbConnection->SetLatencyTimer(1); // 1ms, the minimum
171  std::this_thread::sleep_for(10ms);
172 
174  "[CSickLaserUSB] USB DEVICE S/N:'%s' OPEN SUCCESSFULLY!!!\n",
175  m_serialNumber.c_str());
176  return true;
177  }
178  catch (std::exception& e)
179  {
181  "[CSickLaserUSB] ERROR TRYING TO OPEN USB DEVICE S/N:'%s'\n%s",
182  m_serialNumber.c_str(), e.what());
183  return false;
184  }
185 }
186 
187 /*-------------------------------------------------------------
188  waitContinuousSampleFrame
189 -------------------------------------------------------------*/
191  vector<float>& out_ranges_meters, unsigned char& LMS_status,
192  uint32_t& out_board_timestamp, bool& is_mm_mode)
193 {
194  size_t nRead, nBytesToRead;
195  size_t nFrameBytes = 0;
196  size_t lenghtField;
197  unsigned char buf[2000];
198  buf[2] = buf[3] = 0;
199 
200  while (nFrameBytes < (lenghtField = (6 + (buf[2] | (buf[3] << 8)))) +
201  5 /* for 32bit timestamp + end-flag */)
202  {
203  if (lenghtField > 800)
204  {
205  cout << "#";
206  nFrameBytes = 0; // No es cabecera de trama correcta
207  buf[2] = buf[3] = 0;
208  }
209 
210  if (nFrameBytes < 4)
211  nBytesToRead = 1;
212  else
213  nBytesToRead =
214  (5 /* for 32bit timestamp + end-flag */ + lenghtField) -
215  nFrameBytes;
216 
217  try
218  {
219  nRead = m_usbConnection->ReadSync(buf + nFrameBytes, nBytesToRead);
220  }
221  catch (std::exception& e)
222  {
223  // Disconnected?
225  "[CSickLaserUSB::waitContinuousSampleFrame] Disconnecting due "
226  "to comms error: %s\n",
227  e.what());
229  m_timeStartUI = 0;
230  return false;
231  }
232 
233  if (nRead == 0 && nFrameBytes == 0) return false;
234 
235  if (nRead > 0)
236  {
237  // Lectura OK:
238  // Era la primera?
239  if (nFrameBytes > 1 || (!nFrameBytes && buf[0] == 0x02) ||
240  (nFrameBytes == 1 && buf[1] == 0x80))
241  nFrameBytes += nRead;
242  else
243  {
244  nFrameBytes = 0; // No es cabecera de trama correcta
245  buf[2] = buf[3] = 0;
246  }
247  }
248  }
249 
250  // Frame received
251  // --------------------------------------------------------------------------
252  // | STX | ADDR | L1 | L2 | COM | INF1 | INF2 | DATA | STA | CRC1 | CRC2
253  // |
254  // --------------------------------------------------------------------------
255 
256  // Trama completa:
257  // Checkear que el byte de comando es 0xB0:
258  if (buf[4] != 0xB0) return false;
259 
260  // GET FRAME INFO
261  int info = buf[5] | (buf[6] << 8); // Little Endian
262  int n_points = info & 0x01FF;
263  is_mm_mode = 0 != ((info & 0xC000) >> 14); // 0x00: cm 0x01: mm
264 
265  out_ranges_meters.resize(n_points);
266 
267  // Copiar rangos:
268  short mask = is_mm_mode ? 0x7FFF : 0x1FFF;
269  float meters_scale = is_mm_mode ? 0.001f : 0.01f;
270 
271  for (int i = 0; i < n_points; i++)
272  out_ranges_meters[i] =
273  ((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") <<
282 // endl;
283 #ifdef _WIN32
284  OutputDebugStringA(
285  "[CSickLaserUSB::waitContinuousSampleFrame] bad end flag\n");
286 #endif
287  return false; // Bad CRC
288  }
289 
290  // CRC:
291  const uint16_t CRC = mrpt::system::compute_CRC16(buf, lenghtField - 2);
292  const uint16_t CRC_packet =
293  buf[lenghtField - 2] | (buf[lenghtField - 1] << 8);
294  if (CRC_packet != CRC)
295  {
296  const string s = format(
297  "[CSickLaserUSB::waitContinuousSampleFrame] bad CRC len=%u "
298  "nptns=%u: %i != %i\n",
299  unsigned(lenghtField), unsigned(n_points), CRC_packet, CRC);
300  cerr << s;
301 #ifdef _WIN32
302  OutputDebugStringA(s.c_str());
303 #endif
304  return false; // Bad CRC
305  }
306 
307  // Get USB board timestamp:
308  out_board_timestamp = (uint32_t(buf[nFrameBytes - 5]) << 24) |
309  (uint32_t(buf[nFrameBytes - 4]) << 16) |
310  (uint32_t(buf[nFrameBytes - 3]) << 8) |
311  uint32_t(buf[nFrameBytes - 2]);
312 
313  // All OK
314  return true;
315 }
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:72
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:63
mrpt::system::TTimeStamp now()
A shortcut for system::getCurrentTime.
Definition: datetime.h:87
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:68
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: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:86
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: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).
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:77
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:73
void setScanRangeValidity(const size_t i, const bool val)



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 7d5e6d718 Fri Aug 24 01:51:28 2018 +0200 at lun nov 2 08:35:50 CET 2020