19 #define APPERTURE 4.712385 // in radian <=> 270° 37 m_sensorPose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0),
39 m_beamApperture(.25 *
M_PI / 180.0)
44 CLMS100Eth::~CLMS100Eth()
46 if (m_connected) m_client.close();
51 void CLMS100Eth::initialize()
53 if (!checkIsConnected())
56 "Can't connect to LMS100 Ethernet Sensor check your configuration " 62 void CLMS100Eth::loadConfig_sensorSpecific(
66 C2DRangeFinderAbstract::loadCommonParams(configSource, iniSection);
67 float pose_x, pose_y, pose_z, pose_yaw, pose_pitch, pose_roll;
69 pose_x = configSource.
read_float(iniSection,
"pose_x", 0,
false);
70 pose_y = configSource.
read_float(iniSection,
"pose_y", 0,
false);
71 pose_z = configSource.
read_float(iniSection,
"pose_z", 0,
false);
72 pose_yaw = configSource.
read_float(iniSection,
"pose_yaw", 0,
false);
73 pose_pitch = configSource.
read_float(iniSection,
"pose_pitch", 0,
false);
74 pose_roll = configSource.
read_float(iniSection,
"pose_roll", 0,
false);
76 iniSection,
"ip_address",
"192.168.0.1",
false);
77 m_port = configSource.
read_int(iniSection,
"TCP_port", 2111,
false);
79 configSource.
read_int(iniSection,
string(
"process_rate"), 10,
false);
81 configSource.
read_string(iniSection,
"sensorLabel",
"SICK",
false);
87 bool CLMS100Eth::checkIsConnected(
void)
95 m_client.connect(m_ip, m_port);
97 catch (std::exception& e)
100 "[CLMS100ETH] ERROR TRYING TO OPEN Ethernet DEVICE:\n%s",
109 bool CLMS100Eth::turnOff()
111 if (m_client.isConnected()) m_client.close();
117 bool CLMS100Eth::turnOn()
128 if (checkIsConnected())
133 char msg[] = {
"sMN SetAccessMode 03 F4724744"};
137 size_t read = m_client.readAsync(msgIn, 100, 1000, 1000);
143 if (!read)
return false;
147 "sMN mLMPsetscancfg +2500 +1 +2500 -450000 +2250000"};
151 size_t read = m_client.readAsync(msgIn, 100, 1000, 1000);
157 if (!read)
return false;
161 "sWN LMDscandatacfg 01 00 0 1 0 00 00 0 0 0 0 +1"};
165 size_t read = m_client.readAsync(msgIn, 100, 1000, 1000);
171 if (!read)
return false;
174 char msg[] = {
"sMN LMCstartmeas"};
177 size_t read = m_client.readAsync(msgIn, 100, 1000, 1000);
181 if (!read)
return false;
185 char msg[] = {
"sRN STlms"};
189 size_t read = m_client.readAsync(msgIn, 100, 1000, 1000);
190 std::this_thread::sleep_for(10000ms);
195 if (!read)
return false;
196 }
while (msgIn[11] !=
'7');
200 catch (std::exception& e)
213 void CLMS100Eth::sendCommand(
const char* cmd)
217 m_client.writeAsync(&m_cmd[0], m_cmd.size());
222 void CLMS100Eth::generateCmd(
const char* cmd)
224 if (strlen(cmd) > 995)
229 m_cmd =
format(
"%c%s%c", 0x02, cmd, 0x03);
235 unsigned int idx = 0;
236 unsigned int scanCount = 0;
240 next =
strtok(buff,
" ", &tmp);
242 while (next && scanCount == 0)
248 if (strncmp(&next[1],
"sRA", 3) && strncmp(&next[1],
"sSN", 3))
252 if (strcmp(next,
"LMDscandata"))
return false;
255 if (!strcmp(next,
"1"))
259 else if (!strcmp(next,
"4"))
262 "Contamination error on LMS100: '%s'", next);
270 if (strcmp(next,
"DIST1"))
273 "LMS100 is not configured to send distances.");
282 scanCount = strtoul(next,
nullptr, 16);
288 next =
strtok(
nullptr,
" ", &tmp);
295 outObservation.
maxRange = m_maxRange;
301 for (i = 0; i < scanCount && next; i++, next =
strtok(
nullptr,
" ", &tmp))
304 i,
double(strtoul(next,
nullptr, 16)) / 1000.0);
309 return i >= scanCount;
312 void CLMS100Eth::doProcessSimple(
318 hardwareError =
true;
319 outThereIsObservation =
false;
322 hardwareError =
false;
324 char msg[] = {
"sRN LMDscandata"};
326 char buffIn[16 * 1024];
332 m_client.readAsync(buffIn,
sizeof(buffIn), 40, 40);
334 if (decodeScan(buffIn, outObservation))
337 C2DRangeFinderAbstract::filterByExclusionAreas(outObservation);
338 C2DRangeFinderAbstract::filterByExclusionAngles(outObservation);
340 C2DRangeFinderAbstract::processPreview(outObservation);
342 outThereIsObservation =
true;
343 hardwareError =
false;
347 hardwareError =
true;
348 outThereIsObservation =
false;
354 void CLMS100Eth::doProcess()
357 mrpt::make_aligned_shared<CObservation2DRangeScan>();
360 bool isThereObservation, hwError;
361 doProcessSimple(isThereObservation, *obs, hwError);
367 if (isThereObservation)
369 appendObservation(obs);
381 void CLMS100Eth::setSensorPose(
const CPose3D& _pose) { m_sensorPose = _pose; }
This "software driver" implements the communication protocol for interfacing a SICK LMS100 laser scan...
#define MRPT_LOG_DEBUG(_STRING)
Use: MRPT_LOG_DEBUG("message");
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,...)
#define THROW_EXCEPTION(msg)
float beamAperture
The aperture of each beam, in radians, used to insert "thick" rays in the occupancy grid...
mrpt::system::TTimeStamp getCurrentTime()
Returns the current (UTC) system time.
void setScanRange(const size_t i, const float val)
double DEG2RAD(const double x)
Degrees to radians.
Contains classes for various device interfaces.
float read_float(const std::string §ion, const std::string &name, float defaultValue, bool failIfNotFound=false) const
float stdError
The "sigma" error of the device in meters, used while inserting the scan in an occupancy grid...
int read_int(const std::string §ion, const std::string &name, int defaultValue, bool failIfNotFound=false) const
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...
#define MRPT_LOG_DEBUG_FMT(_FMT_STRING,...)
Use: MRPT_LOG_DEBUG_FMT("i=%u", i);
float getScanRange(const size_t i) const
char * strtok(char *str, const char *strDelimit, char **context) noexcept
An OS-independent method for tokenizing a string.
This namespace contains representation of robot actions and observations.
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...
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...
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
#define MRPT_LOG_ERROR(_STRING)
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.
float aperture
The "aperture" or field-of-view of the range finder, in radians (typically M_PI = 180 degrees)...
mrpt::poses::CPose3D sensorPose
The 6D pose of the sensor on the robot at the moment of starting the scan.
bool rightToLeft
The scanning direction: true=counterclockwise; false=clockwise.
void setScanRangeValidity(const size_t i, const bool val)