Go to the documentation of this file.
33 CSickLaserUSB::CSickLaserUSB() : m_serialNumber(
"LASER001")
45 bool& outThereIsObservation,
48 outThereIsObservation =
false;
49 hardwareError =
false;
58 unsigned char LMS_stat;
59 uint32_t board_timestamp;
66 ranges, LMS_stat, board_timestamp, is_mm_mode))
84 auto AtDO = std::chrono::milliseconds(AtUI - 50);
94 outObservation.
maxRange = is_mm_mode ? 32.7 : 81.0;
100 for (
size_t i = 0; i < ranges.size(); i++)
104 i, (ranges[i] <= outObservation.
maxRange));
113 outThereIsObservation =
true;
121 const std::string& iniSection)
126 configSource.
read_float(iniSection,
"pose_x", 0),
127 configSource.
read_float(iniSection,
"pose_y", 0),
128 configSource.
read_float(iniSection,
"pose_z", 0),
157 std::this_thread::sleep_for(10ms);
159 std::this_thread::sleep_for(10ms);
161 std::this_thread::sleep_for(10ms);
164 "[CSickLaserUSB] USB DEVICE S/N:'%s' OPEN SUCCESSFULLY!!!\n",
168 catch (
const std::exception& e)
171 "[CSickLaserUSB] ERROR TRYING TO OPEN USB DEVICE S/N:'%s'\n%s",
181 vector<float>& out_ranges_meters,
unsigned char& LMS_status,
182 uint32_t& out_board_timestamp,
bool& is_mm_mode)
184 size_t nRead, nBytesToRead;
185 size_t nFrameBytes = 0;
187 unsigned char buf[2000];
190 while (nFrameBytes < (lenghtField = (6 + (buf[2] | (buf[3] << 8)))) +
193 if (lenghtField > 800)
211 catch (
const std::exception& e)
215 "[CSickLaserUSB::waitContinuousSampleFrame] Disconnecting due "
216 "to comms error: %s\n",
223 if (nRead == 0 && nFrameBytes == 0)
return false;
229 if (nFrameBytes > 1 || (!nFrameBytes && buf[0] == 0x02) ||
230 (nFrameBytes == 1 && buf[1] == 0x80))
231 nFrameBytes += nRead;
248 if (buf[4] != 0xB0)
return false;
251 int info = buf[5] | (buf[6] << 8);
252 int n_points = info & 0x01FF;
253 is_mm_mode = 0 != ((info & 0xC000) >> 14);
255 out_ranges_meters.resize(n_points);
258 short mask = is_mm_mode ? 0x7FFF : 0x1FFF;
259 float meters_scale = is_mm_mode ? 0.001f : 0.01f;
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;
266 LMS_status = buf[lenghtField - 3];
269 if (buf[nFrameBytes - 1] != 0x55)
275 "[CSickLaserUSB::waitContinuousSampleFrame] bad end flag\n");
282 const uint16_t CRC_packet =
283 buf[lenghtField - 2] | (buf[lenghtField - 1] << 8);
284 if (CRC_packet != CRC)
287 "[CSickLaserUSB::waitContinuousSampleFrame] bad CRC len=%u "
288 "nptns=%u: %i != %i\n",
289 unsigned(lenghtField),
unsigned(n_points), CRC_packet, CRC);
292 OutputDebugStringA(s.c_str());
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]);
bool turnOn() override
Enables the scanning mode (in this class this has no effect).
std::string sensorLabel
An arbitrary label that can be used to identify the sensor.
void setScanRange(const size_t i, const float val)
#define MRPT_LOG_INFO_FMT(_FMT_STRING,...)
uint32_t m_timeStartUI
Time of the first data packet, for synchronization purposes.
float maxRange
The maximum range allowed by the device, in meters (e.g.
bool rightToLeft
The scanning direction: true=counterclockwise; false=clockwise.
mrpt::system::TTimeStamp m_timeStartTT
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.
std::unique_ptr< mrpt::comms::CInterfaceFTDI > m_usbConnection
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
bool checkControllerIsConnected()
void loadConfig_sensorSpecific(const mrpt::config::CConfigFileBase &configSource, const std::string &iniSection) override
See the class documentation at the top for expected parameters.
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,...
This "software driver" implements the communication protocol for interfacing a SICK LMS2XX laser scan...
void filterByExclusionAngles(mrpt::obs::CObservation2DRangeScan &obs) const
Mark as invalid those ranges in a set of forbiden angle ranges.
mrpt::system::TTimeStamp now()
A shortcut for system::getCurrentTime.
Contains classes for various device interfaces.
Serial and networking devices and utilities.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
void resizeScan(const size_t len)
Resizes all data vectors to allocate a given number of scan rays.
void setScanRangeValidity(const size_t i, const bool val)
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
This namespace contains representation of robot actions and observations.
mrpt::system::TTimeStamp timestamp
The associated UTC time-stamp.
std::string read_string(const std::string §ion, const std::string &name, const std::string &defaultValue, bool failIfNotFound=false) const
float aperture
The "aperture" or field-of-view of the range finder, in radians (typically M_PI = 180 degrees).
This class allows loading and storing values and vectors of different types from a configuration text...
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).
#define MRPT_LOG_ERROR_FMT(_FMT_STRING,...)
poses::CPose3D m_sensorPose
The sensor 6D pose:
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
bool waitContinuousSampleFrame(std::vector< float > &ranges, unsigned char &LMS_status, uint32_t &out_board_timestamp, bool &is_mm_mode)
std::string m_sensorLabel
See CGenericSensor.
float stdError
The "sigma" error of the device in meters, used while inserting the scan in an occupancy grid.
float read_float(const std::string §ion, const std::string &name, float defaultValue, bool failIfNotFound=false) const
bool turnOff() override
Disables the scanning mode (in this class this has no effect).
constexpr double DEG2RAD(const double x)
Degrees to radians
#define IMPLEMENTS_GENERIC_SENSOR(class_name, NameSpace)
This must be inserted in all CGenericSensor classes implementation files:
void processPreview(const mrpt::obs::CObservation2DRangeScan &obs)
Must be called inside the capture method to allow optional GUI preview of scans.
std::string m_serialNumber
mrpt::poses::CPose3D sensorPose
The 6D pose of the sensor on the robot at the moment of starting the scan.
void filterByExclusionAreas(mrpt::obs::CObservation2DRangeScan &obs) const
Mark as invalid those points which (x,y) coordinates fall within the exclusion polygons.
std::string std::string format(std::string_view fmt, ARGS &&... args)
Page generated by Doxygen 1.8.17 for MRPT 2.0.3 at Fri May 15 23:51:15 UTC 2020 | |