33 CSickLaserUSB::CSickLaserUSB() : m_serialNumber(
"LASER001")
54 bool& outThereIsObservation,
57 outThereIsObservation =
false;
58 hardwareError =
false;
67 unsigned char LMS_stat;
75 ranges, LMS_stat, board_timestamp, is_mm_mode))
93 auto AtDO = std::chrono::milliseconds(AtUI - 50);
103 outObservation.
maxRange = is_mm_mode ? 32.7 : 81.0;
109 for (
size_t i = 0; i < ranges.size(); i++)
113 i, (ranges[i] <= outObservation.
maxRange));
122 outThereIsObservation =
true;
135 configSource.
read_float(iniSection,
"pose_x", 0),
136 configSource.
read_float(iniSection,
"pose_y", 0),
137 configSource.
read_float(iniSection,
"pose_z", 0),
166 std::this_thread::sleep_for(10ms);
168 std::this_thread::sleep_for(10ms);
170 std::this_thread::sleep_for(10ms);
173 "[CSickLaserUSB] USB DEVICE S/N:'%s' OPEN SUCCESSFULLY!!!\n",
177 catch (
const std::exception& e)
180 "[CSickLaserUSB] ERROR TRYING TO OPEN USB DEVICE S/N:'%s'\n%s",
190 vector<float>& out_ranges_meters,
unsigned char& LMS_status,
191 uint32_t& out_board_timestamp,
bool& is_mm_mode)
193 size_t nRead, nBytesToRead;
194 size_t nFrameBytes = 0;
196 unsigned char buf[2000];
199 while (nFrameBytes < (lenghtField = (6 + (buf[2] | (buf[3] << 8)))) +
202 if (lenghtField > 800)
220 catch (
const std::exception& e)
224 "[CSickLaserUSB::waitContinuousSampleFrame] Disconnecting due " 225 "to comms error: %s\n",
232 if (nRead == 0 && nFrameBytes == 0)
return false;
238 if (nFrameBytes > 1 || (!nFrameBytes && buf[0] == 0x02) ||
239 (nFrameBytes == 1 && buf[1] == 0x80))
240 nFrameBytes += nRead;
257 if (buf[4] != 0xB0)
return false;
260 int info = buf[5] | (buf[6] << 8);
261 int n_points = info & 0x01FF;
262 is_mm_mode = 0 != ((info & 0xC000) >> 14);
264 out_ranges_meters.resize(n_points);
267 short mask = is_mm_mode ? 0x7FFF : 0x1FFF;
268 float meters_scale = is_mm_mode ? 0.001f : 0.01f;
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;
275 LMS_status = buf[lenghtField - 3];
278 if (buf[nFrameBytes - 1] != 0x55)
284 "[CSickLaserUSB::waitContinuousSampleFrame] bad end flag\n");
292 buf[lenghtField - 2] | (buf[lenghtField - 1] << 8);
293 if (CRC_packet != CRC)
296 "[CSickLaserUSB::waitContinuousSampleFrame] bad CRC len=%u " 297 "nptns=%u: %i != %i\n",
298 unsigned(lenghtField),
unsigned(n_points), CRC_packet, CRC);
301 OutputDebugStringA(
s.c_str());
307 out_board_timestamp = (
uint32_t(buf[nFrameBytes - 5]) << 24) |
308 (
uint32_t(buf[nFrameBytes - 4]) << 16) |
309 (
uint32_t(buf[nFrameBytes - 3]) << 8) |
uint32_t m_timeStartUI
Time of the first data packet, for synchronization purposes.
bool isOpen()
Checks whether the chip has been successfully open.
unsigned __int16 uint16_t
std::string read_string(const std::string §ion, 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...
mrpt::system::TTimeStamp now()
A shortcut for system::getCurrentTime.
Contains classes for various device interfaces.
float read_float(const std::string §ion, const std::string &name, float defaultValue, bool failIfNotFound=false) const
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.
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...
bool checkControllerIsConnected()
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
This namespace contains representation of robot actions and observations.
void OpenBySerialNumber(const std::string &serialNumber)
Open by device serial number.
GLsizei const GLchar ** string
#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.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
mrpt::system::TTimeStamp timestamp
The associated UTC time-stamp.
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).
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.
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:
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.
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
std::string m_serialNumber
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
void setScanRangeValidity(const size_t i, const bool val)