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;