MRPT  2.0.3
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 }
mrpt::hwdrivers::CSickLaserUSB::turnOn
bool turnOn() override
Enables the scanning mode (in this class this has no effect).
Definition: CSickLaserUSB.cpp:139
os.h
mrpt::obs::CObservation::sensorLabel
std::string sensorLabel
An arbitrary label that can be used to identify the sensor.
Definition: CObservation.h:62
mrpt::obs::CObservation2DRangeScan::setScanRange
void setScanRange(const size_t i, const float val)
Definition: CObservation2DRangeScan.cpp:507
MRPT_LOG_INFO_FMT
#define MRPT_LOG_INFO_FMT(_FMT_STRING,...)
Definition: system/COutputLogger.h:463
mrpt::hwdrivers::CSickLaserUSB::m_timeStartUI
uint32_t m_timeStartUI
Time of the first data packet, for synchronization purposes.
Definition: CSickLaserUSB.h:71
mrpt::obs::CObservation2DRangeScan::maxRange
float maxRange
The maximum range allowed by the device, in meters (e.g.
Definition: CObservation2DRangeScan.h:119
mrpt::obs::CObservation2DRangeScan::rightToLeft
bool rightToLeft
The scanning direction: true=counterclockwise; false=clockwise.
Definition: CObservation2DRangeScan.h:116
mrpt::hwdrivers::CSickLaserUSB::m_timeStartTT
mrpt::system::TTimeStamp m_timeStartTT
Definition: CSickLaserUSB.h:72
mrpt::system::compute_CRC16
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
mrpt::hwdrivers::CSickLaserUSB::m_usbConnection
std::unique_ptr< mrpt::comms::CInterfaceFTDI > m_usbConnection
Definition: CSickLaserUSB.h:67
mrpt::obs::CObservation2DRangeScan
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
Definition: CObservation2DRangeScan.h:54
mrpt::hwdrivers::CSickLaserUSB::checkControllerIsConnected
bool checkControllerIsConnected()
Definition: CSickLaserUSB.cpp:147
mrpt::hwdrivers::CSickLaserUSB::loadConfig_sensorSpecific
void loadConfig_sensorSpecific(const mrpt::config::CConfigFileBase &configSource, const std::string &iniSection) override
See the class documentation at the top for expected parameters.
Definition: CSickLaserUSB.cpp:119
mrpt::hwdrivers::CSickLaserUSB::doProcessSimple
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,...
Definition: CSickLaserUSB.cpp:44
crc.h
mrpt::hwdrivers::CSickLaserUSB
This "software driver" implements the communication protocol for interfacing a SICK LMS2XX laser scan...
Definition: CSickLaserUSB.h:62
mrpt::hwdrivers::C2DRangeFinderAbstract::filterByExclusionAngles
void filterByExclusionAngles(mrpt::obs::CObservation2DRangeScan &obs) const
Mark as invalid those ranges in a set of forbiden angle ranges.
Definition: C2DRangeFinderAbstract.cpp:205
mrpt::system::now
mrpt::system::TTimeStamp now()
A shortcut for system::getCurrentTime.
Definition: datetime.h:86
mrpt::hwdrivers::CGenericSensor::ssWorking
@ ssWorking
Definition: CGenericSensor.h:87
mrpt::hwdrivers
Contains classes for various device interfaces.
Definition: C2DRangeFinderAbstract.h:19
mrpt::comms
Serial and networking devices and utilities.
Definition: CClientTCPSocket.h:21
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: BaseAppDataSource.h:15
mrpt::obs::CObservation2DRangeScan::resizeScan
void resizeScan(const size_t len)
Resizes all data vectors to allocate a given number of scan rays.
Definition: CObservation2DRangeScan.cpp:541
mrpt::obs::CObservation2DRangeScan::setScanRangeValidity
void setScanRangeValidity(const size_t i, const bool val)
Definition: CObservation2DRangeScan.cpp:534
mrpt::poses
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CHierarchicalMapMHPartition.h:22
mrpt::hwdrivers::CGenericSensor::m_state
TSensorState m_state
Definition: CGenericSensor.h:148
mrpt::obs
This namespace contains representation of robot actions and observations.
Definition: CParticleFilter.h:17
mrpt::obs::CObservation::timestamp
mrpt::system::TTimeStamp timestamp
The associated UTC time-stamp.
Definition: CObservation.h:60
mrpt::config::CConfigFileBase::read_string
std::string read_string(const std::string &section, const std::string &name, const std::string &defaultValue, bool failIfNotFound=false) const
Definition: CConfigFileBase.cpp:171
mrpt::obs::CObservation2DRangeScan::aperture
float aperture
The "aperture" or field-of-view of the range finder, in radians (typically M_PI = 180 degrees).
Definition: CObservation2DRangeScan.h:114
MRPT_START
#define MRPT_START
Definition: exceptions.h:241
mrpt::config::CConfigFileBase
This class allows loading and storing values and vectors of different types from a configuration text...
Definition: config/CConfigFileBase.h:44
mrpt::hwdrivers::C2DRangeFinderAbstract::loadCommonParams
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).
Definition: C2DRangeFinderAbstract.cpp:119
MRPT_LOG_ERROR_FMT
#define MRPT_LOG_ERROR_FMT(_FMT_STRING,...)
Definition: system/COutputLogger.h:467
mrpt::hwdrivers::CSickLaserUSB::m_sensorPose
poses::CPose3D m_sensorPose
The sensor 6D pose:
Definition: CSickLaserUSB.h:76
mrpt::poses::CPose3D
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
mrpt::hwdrivers::CSickLaserUSB::waitContinuousSampleFrame
bool waitContinuousSampleFrame(std::vector< float > &ranges, unsigned char &LMS_status, uint32_t &out_board_timestamp, bool &is_mm_mode)
Definition: CSickLaserUSB.cpp:180
mrpt::hwdrivers::CGenericSensor::m_sensorLabel
std::string m_sensorLabel
See CGenericSensor.
Definition: CGenericSensor.h:141
mrpt::obs::CObservation2DRangeScan::stdError
float stdError
The "sigma" error of the device in meters, used while inserting the scan in an occupancy grid.
Definition: CObservation2DRangeScan.h:125
mrpt::config::CConfigFileBase::read_float
float read_float(const std::string &section, const std::string &name, float defaultValue, bool failIfNotFound=false) const
Definition: CConfigFileBase.cpp:118
mrpt::hwdrivers::CSickLaserUSB::turnOff
bool turnOff() override
Disables the scanning mode (in this class this has no effect).
Definition: CSickLaserUSB.cpp:143
mrpt::DEG2RAD
constexpr double DEG2RAD(const double x)
Degrees to radians
Definition: core/include/mrpt/core/bits_math.h:47
IMPLEMENTS_GENERIC_SENSOR
#define IMPLEMENTS_GENERIC_SENSOR(class_name, NameSpace)
This must be inserted in all CGenericSensor classes implementation files:
Definition: CGenericSensor.h:314
mrpt::hwdrivers::C2DRangeFinderAbstract::processPreview
void processPreview(const mrpt::obs::CObservation2DRangeScan &obs)
Must be called inside the capture method to allow optional GUI preview of scans.
Definition: C2DRangeFinderAbstract.cpp:211
mrpt::hwdrivers::CSickLaserUSB::m_serialNumber
std::string m_serialNumber
Definition: CSickLaserUSB.h:68
MRPT_END
#define MRPT_END
Definition: exceptions.h:245
CSickLaserUSB.h
M_PIf
#define M_PIf
Definition: common.h:61
CArchive.h
mrpt::obs::CObservation2DRangeScan::sensorPose
mrpt::poses::CPose3D sensorPose
The 6D pose of the sensor on the robot at the moment of starting the scan.
Definition: CObservation2DRangeScan.h:122
mrpt::hwdrivers::C2DRangeFinderAbstract::filterByExclusionAreas
void filterByExclusionAreas(mrpt::obs::CObservation2DRangeScan &obs) const
Mark as invalid those points which (x,y) coordinates fall within the exclusion polygons.
Definition: C2DRangeFinderAbstract.cpp:196
mrpt::format
std::string std::string format(std::string_view fmt, ARGS &&... args)
Definition: format.h:26
hwdrivers-precomp.h



Page generated by Doxygen 1.8.17 for MRPT 2.0.3 at Fri May 15 15:49:54 UTC 2020