20 #define APPERTURE 4.712385 // in radian <=> 270
44 m_sensorPose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0),
46 m_beamApperture(.25 *
M_PI / 180.0)
51 CSICKTim561Eth::~CSICKTim561Eth()
53 if (m_connected) m_client.close();
58 void CSICKTim561Eth::initialize()
60 if (!checkIsConnected())
63 "Cannot connect to SICK Tim561 Ethernet Sensor check your "
70 void CSICKTim561Eth::loadConfig_sensorSpecific(
74 C2DRangeFinderAbstract::loadCommonParams(configSource, iniSection);
75 float pose_x, pose_y, pose_z, pose_yaw, pose_pitch, pose_roll;
77 pose_x = configSource.
read_float(iniSection,
"pose_x", 0,
false);
78 pose_y = configSource.
read_float(iniSection,
"pose_y", 0,
false);
79 pose_z = configSource.
read_float(iniSection,
"pose_z", 0,
false);
80 pose_yaw = configSource.
read_float(iniSection,
"pose_yaw", 0,
false);
81 pose_pitch = configSource.
read_float(iniSection,
"pose_pitch", 0,
false);
82 pose_roll = configSource.
read_float(iniSection,
"pose_roll", 0,
false);
84 iniSection,
"ip_address",
"192.168.0.1",
false);
85 m_port = configSource.
read_int(iniSection,
"TCP_port", 2111,
false);
87 configSource.
read_int(iniSection,
string(
"process_rate"), 15,
false);
89 configSource.
read_string(iniSection,
"sensorLabel",
"SICK",
false);
95 bool CSICKTim561Eth::checkIsConnected(
void)
105 m_client.connect(m_ip, m_port);
107 catch (std::exception& e)
110 "[SICKTIM561ETH] ERROR TRYING TO OPEN Ethernet DEVICE:\n%s",
119 bool CSICKTim561Eth::rebootDev()
123 char msg[] = {
"sMN SetAccessMode 03 F4724744"};
127 size_t read = m_client.readAsync(msgIn, 100, 1000, 1000);
135 "SOPAS - Error setting access mode, unexpected response");
141 char msg[] = {
"sMN mSCreboot"};
145 size_t read = m_client.readAsync(msgIn, 100, 1000, 1000);
153 "SOPAS - Error rebootting scanner, unexpected response.");
160 bool CSICKTim561Eth::turnOff()
162 if (m_client.isConnected()) m_client.close();
168 bool CSICKTim561Eth::turnOn()
189 if (checkIsConnected())
197 char msg[] = {
"sRIO"};
201 size_t read = m_client.readAsync(msgIn, 100, 1000, 1000);
209 "SOPAS - Error reading variable 'DeviceIdent'.");
216 char msg[] = {
"sRN SerialNumber"};
220 size_t read = m_client.readAsync(msgIn, 100, 1000, 1000);
228 "SOPAS - Error reading variable 'SerialNumber'.");
235 char msg[] = {
"sRN FirmwareVersion"};
239 size_t read = m_client.readAsync(msgIn, 100, 1000, 1000);
247 "SOPAS - Error reading variable 'FirmwareVersion'.");
254 char msg[] = {
"sRN SCdevicestate"};
258 size_t read = m_client.readAsync(msgIn, 100, 1000, 1000);
266 "SOPAS - Error reading variable 'devicestate'.");
329 char msg[] = {
"sEN LMDscandata 1"};
332 size_t read = m_client.readAsync(msgIn, 100, 1000, 1000);
360 catch (std::exception& e)
373 void CSICKTim561Eth::sendCommand(
const char* cmd)
378 m_client.writeAsync(&m_cmd[0], m_cmd.size());
382 void CSICKTim561Eth::generateCmd(
const char* cmd)
384 if (strlen(cmd) > 995)
390 m_cmd =
format(
"%c%s%c%c", 0x02, cmd, 0x03, 0);
393 bool CSICKTim561Eth::decodeScan(
397 unsigned int idx = 0;
398 unsigned int scanCount = 0;
401 next =
strtok(buff,
" ", &tmp);
403 while (next && scanCount == 0)
409 if (strncmp(&next[1],
"sRA", 3) && strncmp(&next[1],
"sSN", 3))
415 if (strcmp(next,
"LMDscandata"))
421 if (strcmp(next,
"1"))
425 else if (strcmp(next,
"0"))
436 if (strcmp(next,
"DIST1"))
439 "TIM561 is not configured to send distances");
445 scanCount = strtoul(next,
nullptr, 16);
451 next =
strtok(
nullptr,
" ", &tmp);
458 outObservation.
maxRange = m_maxRange;
464 for (i = 0; i < scanCount && next; i++, next =
strtok(
nullptr,
" ", &tmp))
467 i,
double(strtoul(next,
nullptr, 16)) / 1000.0);
472 return i >= scanCount;
475 void CSICKTim561Eth::doProcessSimple(
481 hardwareError =
true;
482 outThereIsObservation =
false;
485 hardwareError =
false;
487 char msg[] = {
"sRN LMDscandata"};
489 char buffIn[16 * 1024];
491 m_client.readAsync(buffIn,
sizeof(buffIn), 40, 40);
493 if (decodeScan(buffIn, outObservation))
496 C2DRangeFinderAbstract::filterByExclusionAreas(outObservation);
497 C2DRangeFinderAbstract::filterByExclusionAngles(outObservation);
499 C2DRangeFinderAbstract::processPreview(outObservation);
502 outThereIsObservation =
true;
503 hardwareError =
false;
507 hardwareError =
true;
508 outThereIsObservation =
false;
513 void CSICKTim561Eth::doProcess()
516 mrpt::make_aligned_shared<CObservation2DRangeScan>();
519 bool isThereObservation, hwError;
520 doProcessSimple(isThereObservation, *obs, hwError);
532 if (isThereObservation)
534 appendObservation(obs);
546 void CSICKTim561Eth::setSensorPose(
const CPose3D& _pose)
548 m_sensorPose = _pose;