Go to the documentation of this file.
19 #define APPERTURE 4.712385 // in radian <=> 270°
35 m_sensorPose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0),
36 m_beamApperture(.25 *
M_PI / 180.0)
41 CLMS100Eth::~CLMS100Eth()
43 if (m_connected) m_client.close();
48 if (!checkIsConnected())
51 "Can't connect to LMS100 Ethernet Sensor check your configuration "
57 void CLMS100Eth::loadConfig_sensorSpecific(
59 const std::string& iniSection)
61 C2DRangeFinderAbstract::loadCommonParams(configSource, iniSection);
62 float pose_x, pose_y, pose_z, pose_yaw, pose_pitch, pose_roll;
64 pose_x = configSource.
read_float(iniSection,
"pose_x", 0,
false);
65 pose_y = configSource.
read_float(iniSection,
"pose_y", 0,
false);
66 pose_z = configSource.
read_float(iniSection,
"pose_z", 0,
false);
67 pose_yaw = configSource.
read_float(iniSection,
"pose_yaw", 0,
false);
68 pose_pitch = configSource.
read_float(iniSection,
"pose_pitch", 0,
false);
69 pose_roll = configSource.
read_float(iniSection,
"pose_roll", 0,
false);
71 iniSection,
"ip_address",
"192.168.0.1",
false);
72 m_port = configSource.
read_int(iniSection,
"TCP_port", 2111,
false);
74 configSource.
read_int(iniSection,
string(
"process_rate"), 10,
false);
76 configSource.
read_string(iniSection,
"sensorLabel",
"SICK",
false);
82 bool CLMS100Eth::checkIsConnected()
90 m_client.connect(m_ip, m_port);
92 catch (
const std::exception& e)
95 "[CLMS100ETH] ERROR TRYING TO OPEN Ethernet DEVICE:\n%s",
104 bool CLMS100Eth::turnOff()
106 if (m_client.isConnected()) m_client.close();
112 bool CLMS100Eth::turnOn()
123 if (checkIsConnected())
128 char msg[] = {
"sMN SetAccessMode 03 F4724744"};
132 size_t read = m_client.readAsync(msgIn, 100, 1000, 1000);
138 if (!read)
return false;
142 "sMN mLMPsetscancfg +2500 +1 +2500 -450000 +2250000"};
146 size_t read = m_client.readAsync(msgIn, 100, 1000, 1000);
152 if (!read)
return false;
156 "sWN LMDscandatacfg 01 00 0 1 0 00 00 0 0 0 0 +1"};
160 size_t read = m_client.readAsync(msgIn, 100, 1000, 1000);
166 if (!read)
return false;
169 char msg[] = {
"sMN LMCstartmeas"};
172 size_t read = m_client.readAsync(msgIn, 100, 1000, 1000);
176 if (!read)
return false;
180 char msg[] = {
"sRN STlms"};
184 size_t read = m_client.readAsync(msgIn, 100, 1000, 1000);
185 std::this_thread::sleep_for(10000ms);
190 if (!read)
return false;
191 }
while (msgIn[11] !=
'7');
195 catch (
const std::exception& e)
208 void CLMS100Eth::sendCommand(
const char*
cmd)
212 m_client.writeAsync(&m_cmd[0], m_cmd.size());
217 void CLMS100Eth::generateCmd(
const char*
cmd)
219 if (strlen(
cmd) > 995)
224 m_cmd =
format(
"%c%s%c", 0x02,
cmd, 0x03);
230 unsigned int idx = 0;
231 unsigned int scanCount = 0;
235 next =
strtok(buff,
" ", &tmp);
237 while (next && scanCount == 0)
243 if (strncmp(&next[1],
"sRA", 3) != 0 &&
244 strncmp(&next[1],
"sSN", 3) != 0)
248 if (strcmp(next,
"LMDscandata"))
return false;
251 if (!strcmp(next,
"1"))
255 else if (!strcmp(next,
"4"))
258 "Contamination error on LMS100: '%s'", next);
266 if (strcmp(next,
"DIST1") != 0)
269 "LMS100 is not configured to send distances.");
278 scanCount = strtoul(next,
nullptr, 16);
284 next =
strtok(
nullptr,
" ", &tmp);
291 outObservation.
maxRange = m_maxRange;
297 for (i = 0; i < scanCount && next; i++, next =
strtok(
nullptr,
" ", &tmp))
300 i,
double(strtoul(next,
nullptr, 16)) / 1000.0);
305 return i >= scanCount;
308 void CLMS100Eth::doProcessSimple(
314 hardwareError =
true;
315 outThereIsObservation =
false;
318 hardwareError =
false;
320 char msg[] = {
"sRN LMDscandata"};
322 char buffIn[16 * 1024];
328 m_client.readAsync(buffIn,
sizeof(buffIn), 40, 40);
330 if (decodeScan(buffIn, outObservation))
333 C2DRangeFinderAbstract::filterByExclusionAreas(outObservation);
334 C2DRangeFinderAbstract::filterByExclusionAngles(outObservation);
336 C2DRangeFinderAbstract::processPreview(outObservation);
338 outThereIsObservation =
true;
339 hardwareError =
false;
343 hardwareError =
true;
344 outThereIsObservation =
false;
350 void CLMS100Eth::doProcess()
353 std::make_shared<CObservation2DRangeScan>();
356 bool isThereObservation, hwError;
357 doProcessSimple(isThereObservation, *obs, hwError);
363 if (isThereObservation)
365 appendObservation(obs);
377 void CLMS100Eth::setSensorPose(
const CPose3D& _pose) { m_sensorPose = _pose; }
std::string sensorLabel
An arbitrary label that can be used to identify the sensor.
void setScanRange(const size_t i, const float val)
float maxRange
The maximum range allowed by the device, in meters (e.g.
bool rightToLeft
The scanning direction: true=counterclockwise; false=clockwise.
#define MRPT_LOG_DEBUG(_STRING)
Use: MRPT_LOG_DEBUG("message");
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
const float & getScanRange(const size_t i) const
The range values of the scan, in meters.
Contains classes for various device interfaces.
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.
#define THROW_EXCEPTION(msg)
void setScanRangeValidity(const size_t i, const bool val)
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
#define MRPT_LOG_DEBUG_FMT(_FMT_STRING,...)
Use: MRPT_LOG_DEBUG_FMT("i=%u", i);
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
std::shared_ptr< mrpt::obs ::CObservation2DRangeScan > Ptr
int read_int(const std::string §ion, const std::string &name, int 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...
char * strtok(char *str, const char *strDelimit, char **context) noexcept
An OS-independent method for tokenizing a string.
#define MRPT_LOG_ERROR_FMT(_FMT_STRING,...)
#define MRPT_LOG_ERROR(_STRING)
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
TCLAP::CmdLine cmd("system_control_rate_timer_example")
float beamAperture
The aperture of each beam, in radians, used to insert "thick" rays in the occupancy grid.
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
This "software driver" implements the communication protocol for interfacing a SICK LMS100 laser scan...
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:
mrpt::system::TTimeStamp getCurrentTime()
Returns the current (UTC) system time.
mrpt::poses::CPose3D sensorPose
The 6D pose of the sensor on the robot at the moment of starting the scan.
app initialize(argc, argv)
std::string std::string format(std::string_view fmt, ARGS &&... args)
Page generated by Doxygen 1.8.17 for MRPT 2.0.4 at Fri Jul 17 08:43:33 UTC 2020 | |