20 #define APPERTURE 4.712385 // in radian <=> 270 42 m_sensorPose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0),
43 m_beamApperture(.25 *
M_PI / 180.0)
48 CSICKTim561Eth::~CSICKTim561Eth()
50 if (m_connected) m_client.close();
55 if (!checkIsConnected())
58 "Cannot connect to SICK Tim561 Ethernet Sensor check your " 65 void CSICKTim561Eth::loadConfig_sensorSpecific(
67 const std::string& iniSection)
69 C2DRangeFinderAbstract::loadCommonParams(configSource, iniSection);
70 float pose_x, pose_y, pose_z, pose_yaw, pose_pitch, pose_roll;
72 pose_x = configSource.
read_float(iniSection,
"pose_x", 0,
false);
73 pose_y = configSource.
read_float(iniSection,
"pose_y", 0,
false);
74 pose_z = configSource.
read_float(iniSection,
"pose_z", 0,
false);
75 pose_yaw = configSource.
read_float(iniSection,
"pose_yaw", 0,
false);
76 pose_pitch = configSource.
read_float(iniSection,
"pose_pitch", 0,
false);
77 pose_roll = configSource.
read_float(iniSection,
"pose_roll", 0,
false);
79 iniSection,
"ip_address",
"192.168.0.1",
false);
80 m_port = configSource.
read_int(iniSection,
"TCP_port", 2111,
false);
82 configSource.
read_int(iniSection,
string(
"process_rate"), 15,
false);
84 configSource.
read_string(iniSection,
"sensorLabel",
"SICK",
false);
90 bool CSICKTim561Eth::checkIsConnected()
100 m_client.connect(m_ip, m_port);
102 catch (
const std::exception& e)
105 "[SICKTIM561ETH] ERROR TRYING TO OPEN Ethernet DEVICE:\n%s",
114 bool CSICKTim561Eth::rebootDev()
118 char msg[] = {
"sMN SetAccessMode 03 F4724744"};
122 size_t read = m_client.readAsync(msgIn, 100, 1000, 1000);
130 "SOPAS - Error setting access mode, unexpected response");
136 char msg[] = {
"sMN mSCreboot"};
140 size_t read = m_client.readAsync(msgIn, 100, 1000, 1000);
148 "SOPAS - Error rebootting scanner, unexpected response.");
155 bool CSICKTim561Eth::turnOff()
157 if (m_client.isConnected()) m_client.close();
163 bool CSICKTim561Eth::turnOn()
184 if (checkIsConnected())
192 char msg[] = {
"sRIO"};
196 size_t read = m_client.readAsync(msgIn, 100, 1000, 1000);
204 "SOPAS - Error reading variable 'DeviceIdent'.");
211 char msg[] = {
"sRN SerialNumber"};
215 size_t read = m_client.readAsync(msgIn, 100, 1000, 1000);
223 "SOPAS - Error reading variable 'SerialNumber'.");
230 char msg[] = {
"sRN FirmwareVersion"};
234 size_t read = m_client.readAsync(msgIn, 100, 1000, 1000);
242 "SOPAS - Error reading variable 'FirmwareVersion'.");
249 char msg[] = {
"sRN SCdevicestate"};
253 size_t read = m_client.readAsync(msgIn, 100, 1000, 1000);
261 "SOPAS - Error reading variable 'devicestate'.");
324 char msg[] = {
"sEN LMDscandata 1"};
327 size_t read = m_client.readAsync(msgIn, 100, 1000, 1000);
355 catch (
const std::exception& e)
368 void CSICKTim561Eth::sendCommand(
const char*
cmd)
373 m_client.writeAsync(&m_cmd[0], m_cmd.size());
377 void CSICKTim561Eth::generateCmd(
const char*
cmd)
379 if (strlen(
cmd) > 995)
385 m_cmd =
format(
"%c%s%c%c", 0x02,
cmd, 0x03, 0);
388 bool CSICKTim561Eth::decodeScan(
392 unsigned int idx = 0;
393 unsigned int scanCount = 0;
396 next =
strtok(buff,
" ", &tmp);
398 while (next && scanCount == 0)
404 if (strncmp(&next[1],
"sRA", 3) && strncmp(&next[1],
"sSN", 3))
410 if (strcmp(next,
"LMDscandata"))
416 if (strcmp(next,
"1"))
420 else if (strcmp(next,
"0"))
431 if (strcmp(next,
"DIST1"))
434 "TIM561 is not configured to send distances");
440 scanCount = strtoul(next,
nullptr, 16);
446 next =
strtok(
nullptr,
" ", &tmp);
453 outObservation.
maxRange = m_maxRange;
459 for (i = 0; i < scanCount && next; i++, next =
strtok(
nullptr,
" ", &tmp))
462 i,
double(strtoul(next,
nullptr, 16)) / 1000.0);
467 return i >= scanCount;
470 void CSICKTim561Eth::doProcessSimple(
476 hardwareError =
true;
477 outThereIsObservation =
false;
480 hardwareError =
false;
482 char msg[] = {
"sRN LMDscandata"};
484 char buffIn[16 * 1024];
486 m_client.readAsync(buffIn,
sizeof(buffIn), 40, 40);
488 if (decodeScan(buffIn, outObservation))
491 C2DRangeFinderAbstract::filterByExclusionAreas(outObservation);
492 C2DRangeFinderAbstract::filterByExclusionAngles(outObservation);
494 C2DRangeFinderAbstract::processPreview(outObservation);
497 outThereIsObservation =
true;
498 hardwareError =
false;
502 hardwareError =
true;
503 outThereIsObservation =
false;
508 void CSICKTim561Eth::doProcess()
511 std::make_shared<CObservation2DRangeScan>();
514 bool isThereObservation, hwError;
515 doProcessSimple(isThereObservation, *obs, hwError);
527 if (isThereObservation)
529 appendObservation(obs);
541 void CSICKTim561Eth::setSensorPose(
const CPose3D& _pose)
543 m_sensorPose = _pose;
#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,...)
app initialize(argc, argv)
#define THROW_EXCEPTION(msg)
std::string std::string format(std::string_view fmt, ARGS &&... args)
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)
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);
constexpr double DEG2RAD(const double x)
Degrees to radians.
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.
#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...
TCLAP::CmdLine cmd("system_control_rate_timer_example")
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.
const float & getScanRange(const size_t i) const
The range values of the scan, in meters.
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)