| 1 |
/* +------------------------------------------------------------------------+ |
| 2 |
| Mobile Robot Programming Toolkit (MRPT) | |
| 3 |
| https://www.mrpt.org/ | |
| 4 |
| | |
| 5 |
| Copyright (c) 2005-2026, Individual contributors, see AUTHORS file | |
| 6 |
| See: https://www.mrpt.org/Authors - All rights reserved. | |
| 7 |
| Released under BSD License. See: https://www.mrpt.org/License | |
| 8 |
+------------------------------------------------------------------------+ */ |
| 9 |
|
| 10 |
#include "hwdrivers-precomp.h" // Precompiled headers |
| 11 |
// |
| 12 |
#include <mrpt/comms/CClientTCPSocket.h> |
| 13 |
#include <mrpt/comms/CSerialPort.h> |
| 14 |
#include <mrpt/hwdrivers/CHokuyoURG.h> |
| 15 |
#include <mrpt/opengl/CAxis.h> |
| 16 |
#include <mrpt/opengl/CPlanarLaserScan.h> // in library mrpt-maps |
| 17 |
#include <mrpt/system/CTimeLogger.h> |
| 18 |
#include <mrpt/system/os.h> |
| 19 |
|
| 20 |
#include <chrono> |
| 21 |
#include <thread> |
| 22 |
|
| 23 |
IMPLEMENTS_GENERIC_SENSOR(CHokuyoURG, mrpt::hwdrivers) |
| 24 |
|
| 25 |
using namespace mrpt::obs; |
| 26 |
using namespace mrpt::hwdrivers; |
| 27 |
using namespace mrpt::comms; |
| 28 |
using namespace mrpt::system; |
| 29 |
using namespace mrpt::opengl; |
| 30 |
using namespace std; |
| 31 |
|
| 32 |
const int MINIMUM_PACKETS_TO_SET_TIMESTAMP_REFERENCE = 10; |
| 33 |
|
| 34 |
CHokuyoURG::CHokuyoURG() : m_rx_buffer(40000) { m_sensorLabel = "Hokuyo"; } |
| 35 |
|
| 36 |
CHokuyoURG::~CHokuyoURG() |
| 37 |
{ |
| 38 |
try |
| 39 |
{ |
| 40 |
m_win.reset(); |
| 41 |
closeStreamConnection(); |
| 42 |
} |
| 43 |
catch (const std::exception& e) |
| 44 |
{ |
| 45 |
std::cerr << "[~CHokuyoURG] Exception:\n" << mrpt::exception_to_str(e); |
| 46 |
} |
| 47 |
} |
| 48 |
|
| 49 |
void CHokuyoURG::closeStreamConnection() |
| 50 |
{ |
| 51 |
if (m_stream) |
| 52 |
{ |
| 53 |
turnOff(); |
| 54 |
m_stream.reset(); |
| 55 |
} |
| 56 |
} |
| 57 |
|
| 58 |
void CHokuyoURG::sendCmd(const char* str) |
| 59 |
{ |
| 60 |
MRPT_START |
| 61 |
ASSERT_(str != nullptr); |
| 62 |
ASSERT_(m_stream != nullptr); |
| 63 |
const size_t N = strlen(str); |
| 64 |
const size_t nWriten = m_stream->Write(str, N); |
| 65 |
ASSERT_EQUAL_(nWriten, N); |
| 66 |
|
| 67 |
MRPT_LOG_DEBUG_STREAM("[Hokuyo] sendCmd(): `" << str << "`"); |
| 68 |
|
| 69 |
m_lastSentMeasCmd = std::string(str); // for echo verification |
| 70 |
MRPT_END |
| 71 |
} |
| 72 |
|
| 73 |
void CHokuyoURG::doProcessSimple( |
| 74 |
bool& outThereIsObservation, |
| 75 |
mrpt::obs::CObservation2DRangeScan& outObservation, |
| 76 |
bool& hardwareError) |
| 77 |
{ |
| 78 |
#if 0 |
| 79 |
static mrpt::system::CTimeLogger timLogger("CHokuyoURG"); |
| 80 |
mrpt::system::CTimeLoggerEntry tle(timLogger, "doProcessSimple"); |
| 81 |
#endif |
| 82 |
|
| 83 |
outThereIsObservation = false; |
| 84 |
hardwareError = false; |
| 85 |
|
| 86 |
// Bound? If not, connect to the device and set it up (again) in |
| 87 |
// continuous read mode: |
| 88 |
if (!ensureStreamIsOpen()) |
| 89 |
{ |
| 90 |
m_timeStartUI = 0; |
| 91 |
m_timeStartSynchDelay = 0; |
| 92 |
hardwareError = true; |
| 93 |
return; |
| 94 |
} |
| 95 |
|
| 96 |
// Wait for a message: |
| 97 |
int nRanges = m_lastRange - m_firstRange + 1; |
| 98 |
int expectedSize = nRanges * 3 + 4; |
| 99 |
if (m_intensity) expectedSize += nRanges * 3; |
| 100 |
|
| 101 |
m_rcv_data.clear(); |
| 102 |
m_rcv_data.reserve(expectedSize + 1000); |
| 103 |
|
| 104 |
m_state = ssWorking; |
| 105 |
if (!parseResponse(false)) |
| 106 |
{ |
| 107 |
if (!internal_notifyNoScanReceived()) |
| 108 |
{ |
| 109 |
// It seems the sensor needs to be reset (?), let this know |
| 110 |
// to the caller: |
| 111 |
m_state = ssError; |
| 112 |
hardwareError = true; |
| 113 |
|
| 114 |
// And on our side, close the connection to ensure initialization |
| 115 |
// is called again to set-up the laser in the next call to |
| 116 |
// ensureStreamIsOpen() above. |
| 117 |
closeStreamConnection(); |
| 118 |
|
| 119 |
return; |
| 120 |
} |
| 121 |
|
| 122 |
// No new data |
| 123 |
return; |
| 124 |
} |
| 125 |
|
| 126 |
// DECODE: |
| 127 |
if (m_rcv_status0 != '0' && m_rcv_status0 != '9') |
| 128 |
{ |
| 129 |
hardwareError = true; |
| 130 |
return; |
| 131 |
} |
| 132 |
|
| 133 |
// ----------------------------------------------- |
| 134 |
// Extract the observation: |
| 135 |
// ----------------------------------------------- |
| 136 |
outObservation.timestamp = mrpt::Clock::now(); |
| 137 |
|
| 138 |
if ((size_t)expectedSize != m_rcv_data.size()) |
| 139 |
{ |
| 140 |
MRPT_LOG_ERROR_STREAM( |
| 141 |
"[CHokuyoURG::doProcess] ERROR: Expected " << expectedSize << " data bytes, received " |
| 142 |
<< m_rcv_data.size() << "instead!"); |
| 143 |
hardwareError = true; |
| 144 |
return; |
| 145 |
} |
| 146 |
// Delay the sync of timestamps due to instability in the constant rate |
| 147 |
// during the first few packets. |
| 148 |
bool do_timestamp_sync = !m_disable_firmware_timestamp; |
| 149 |
if (do_timestamp_sync && m_timeStartSynchDelay < MINIMUM_PACKETS_TO_SET_TIMESTAMP_REFERENCE) |
| 150 |
{ |
| 151 |
do_timestamp_sync = false; |
| 152 |
m_timeStartSynchDelay++; |
| 153 |
} |
| 154 |
|
| 155 |
if (do_timestamp_sync) |
| 156 |
{ |
| 157 |
// Extract the timestamp of the sensor: |
| 158 |
uint32_t nowUI = ((m_rcv_data[0] - 0x30) << 18) + ((m_rcv_data[1] - 0x30) << 12) + |
| 159 |
((m_rcv_data[2] - 0x30) << 6) + (m_rcv_data[3] - 0x30); |
| 160 |
|
| 161 |
uint32_t AtUI = 0; |
| 162 |
if (m_timeStartUI == 0) |
| 163 |
{ |
| 164 |
m_timeStartUI = nowUI; |
| 165 |
m_timeStartTT = mrpt::Clock::now(); |
| 166 |
} |
| 167 |
else |
| 168 |
AtUI = nowUI - m_timeStartUI; |
| 169 |
|
| 170 |
auto AtDO = std::chrono::milliseconds(AtUI); |
| 171 |
outObservation.timestamp = m_timeStartTT + AtDO; |
| 172 |
} |
| 173 |
|
| 174 |
// And the scan ranges: |
| 175 |
outObservation.rightToLeft = true; |
| 176 |
|
| 177 |
outObservation.aperture = nRanges * 2 * M_PI / m_sensor_info.scans_per_360deg; |
| 178 |
|
| 179 |
outObservation.maxRange = m_sensor_info.d_max; |
| 180 |
outObservation.stdError = 0.010f; |
| 181 |
outObservation.sensorPose = m_sensorPose; |
| 182 |
outObservation.sensorLabel = m_sensorLabel; |
| 183 |
|
| 184 |
outObservation.resizeScan(nRanges); |
| 185 |
char* ptr = (char*)&m_rcv_data[4]; |
| 186 |
|
| 187 |
if (m_intensity) outObservation.setScanHasIntensity(true); |
| 188 |
|
| 189 |
for (int i = 0; i < nRanges; i++) |
| 190 |
{ |
| 191 |
int b1 = (*ptr++) - 0x30; |
| 192 |
int b2 = (*ptr++) - 0x30; |
| 193 |
int b3 = (*ptr++) - 0x30; |
| 194 |
|
| 195 |
int range_mm = ((b1 << 12) | (b2 << 6) | b3); |
| 196 |
|
| 197 |
outObservation.setScanRange(i, range_mm * 0.001f); |
| 198 |
outObservation.setScanRangeValidity( |
| 199 |
i, range_mm >= 20 && (outObservation.getScanRange(i) <= outObservation.maxRange)); |
| 200 |
|
| 201 |
if (m_intensity) |
| 202 |
{ |
| 203 |
int b4 = (*ptr++) - 0x30; |
| 204 |
int b5 = (*ptr++) - 0x30; |
| 205 |
int b6 = (*ptr++) - 0x30; |
| 206 |
outObservation.setScanIntensity(i, ((b4 << 12) | (b5 << 6) | b6)); |
| 207 |
} |
| 208 |
} |
| 209 |
|
| 210 |
// Do filter: |
| 211 |
C2DRangeFinderAbstract::filterByExclusionAreas(outObservation); |
| 212 |
C2DRangeFinderAbstract::filterByExclusionAngles(outObservation); |
| 213 |
// Do show preview: |
| 214 |
C2DRangeFinderAbstract::processPreview(outObservation); |
| 215 |
|
| 216 |
outThereIsObservation = true; |
| 217 |
internal_notifyGoodScanNow(); |
| 218 |
} |
| 219 |
|
| 220 |
/*------------------------------------------------------------- |
| 221 |
loadConfig_sensorSpecific |
| 222 |
-------------------------------------------------------------*/ |
| 223 |
void CHokuyoURG::loadConfig_sensorSpecific( |
| 224 |
const mrpt::config::CConfigFileBase& c, const std::string& s) |
| 225 |
{ |
| 226 |
m_reduced_fov = DEG2RAD(c.read_float(s, "reduced_fov", 0)), |
| 227 |
|
| 228 |
m_motorSpeed_rpm = c.read_int(s, "HOKUYO_motorSpeed_rpm", 0); |
| 229 |
m_sensorPose.setFromValues( |
| 230 |
c.read_float(s, "pose_x", 0), c.read_float(s, "pose_y", 0), c.read_float(s, "pose_z", 0), |
| 231 |
DEG2RAD(c.read_float(s, "pose_yaw", 0)), DEG2RAD(c.read_float(s, "pose_pitch", 0)), |
| 232 |
DEG2RAD(c.read_float(s, "pose_roll", 0))); |
| 233 |
|
| 234 |
m_highSensMode = c.read_bool(s, "HOKUYO_HS_mode", m_highSensMode); |
| 235 |
|
| 236 |
#ifdef _WIN32 |
| 237 |
const char* comVarName = "COM_port_WIN"; |
| 238 |
#else |
| 239 |
const char* comVarName = "COM_port_LIN"; |
| 240 |
#endif |
| 241 |
|
| 242 |
m_com_port = c.read_string(s, comVarName, m_com_port); |
| 243 |
m_ip_dir = c.read_string(s, "IP_DIR", m_ip_dir); |
| 244 |
m_port_dir = c.read_int(s, "PORT_DIR", m_port_dir); |
| 245 |
|
| 246 |
ASSERTMSG_( |
| 247 |
!m_com_port.empty() || !m_ip_dir.empty(), |
| 248 |
"Either COM_port or IP_DIR must be defined in the configuration file!"); |
| 249 |
ASSERTMSG_( |
| 250 |
m_com_port.empty() || m_ip_dir.empty(), |
| 251 |
"Both COM_port and IP_DIR set! Please, define only one of them."); |
| 252 |
if (!m_ip_dir.empty()) |
| 253 |
{ |
| 254 |
ASSERTMSG_( |
| 255 |
m_port_dir, |
| 256 |
"A TCP/IP port number `PORT_DIR` must be specified for Ethernet " |
| 257 |
"connection"); |
| 258 |
} |
| 259 |
|
| 260 |
m_disable_firmware_timestamp = |
| 261 |
c.read_bool(s, "disable_firmware_timestamp", m_disable_firmware_timestamp); |
| 262 |
m_intensity = c.read_bool(s, "intensity", m_intensity), |
| 263 |
|
| 264 |
MRPT_LOAD_HERE_CONFIG_VAR(scan_interval, int, m_scan_interval, c, s); |
| 265 |
MRPT_LOAD_HERE_CONFIG_VAR(comms_timeout_ms, int, m_comms_timeout_ms, c, s); |
| 266 |
MRPT_LOAD_HERE_CONFIG_VAR(comms_between_timeout_ms, int, m_comms_between_timeout_ms, c, s); |
| 267 |
|
| 268 |
// Parent options: |
| 269 |
C2DRangeFinderAbstract::loadCommonParams(c, s); |
| 270 |
} |
| 271 |
|
| 272 |
/*------------------------------------------------------------- |
| 273 |
turnOn |
| 274 |
-------------------------------------------------------------*/ |
| 275 |
bool CHokuyoURG::turnOn() |
| 276 |
{ |
| 277 |
MRPT_START |
| 278 |
|
| 279 |
// Bound? |
| 280 |
if (!ensureStreamIsOpen()) return false; |
| 281 |
|
| 282 |
// If we are over a serial link, set it up: |
| 283 |
if (m_ip_dir.empty()) |
| 284 |
{ |
| 285 |
auto* COM = dynamic_cast<CSerialPort*>(m_stream.get()); |
| 286 |
|
| 287 |
if (COM != nullptr) |
| 288 |
{ |
| 289 |
// It is a COM: |
| 290 |
COM->setConfig(19200); |
| 291 |
COM->setTimeouts(m_comms_timeout_ms, 0, m_comms_timeout_ms, 0, 0); |
| 292 |
|
| 293 |
// Assure the laser is off and quiet: |
| 294 |
switchLaserOff(); |
| 295 |
std::this_thread::sleep_for(10ms); |
| 296 |
|
| 297 |
COM->purgeBuffers(); |
| 298 |
std::this_thread::sleep_for(10ms); |
| 299 |
|
| 300 |
COM->setConfig(115200); |
| 301 |
switchLaserOff(); |
| 302 |
std::this_thread::sleep_for(10ms); |
| 303 |
COM->purgeBuffers(); |
| 304 |
std::this_thread::sleep_for(10ms); |
| 305 |
COM->setConfig(19200); |
| 306 |
} |
| 307 |
|
| 308 |
if (COM != nullptr) |
| 309 |
{ |
| 310 |
// Set 115200 baud rate: |
| 311 |
setHighBaudrate(); |
| 312 |
enableSCIP20(); |
| 313 |
COM->setConfig(115200); |
| 314 |
} |
| 315 |
} |
| 316 |
else |
| 317 |
{ |
| 318 |
auto* COM = dynamic_cast<CClientTCPSocket*>(m_stream.get()); |
| 319 |
|
| 320 |
if (COM != nullptr) |
| 321 |
{ |
| 322 |
// Assure the laser is off and quiet: |
| 323 |
switchLaserOff(); |
| 324 |
std::this_thread::sleep_for(10ms); |
| 325 |
|
| 326 |
purgeBuffers(); |
| 327 |
std::this_thread::sleep_for(10ms); |
| 328 |
|
| 329 |
switchLaserOff(); |
| 330 |
std::this_thread::sleep_for(10ms); |
| 331 |
purgeBuffers(); |
| 332 |
} |
| 333 |
} |
| 334 |
|
| 335 |
if (!enableSCIP20()) return false; |
| 336 |
|
| 337 |
// Turn on the laser: |
| 338 |
if (!switchLaserOn()) return false; |
| 339 |
|
| 340 |
// Set the motor speed: |
| 341 |
if (m_motorSpeed_rpm) |
| 342 |
if (!setMotorSpeed(m_motorSpeed_rpm)) return false; |
| 343 |
|
| 344 |
// Set HS mode: |
| 345 |
setHighSensitivityMode(m_highSensMode); |
| 346 |
|
| 347 |
// Display sensor information: |
| 348 |
if (!displaySensorInfo(&m_sensor_info)) return false; |
| 349 |
|
| 350 |
// Set for scanning angles: |
| 351 |
m_firstRange = m_sensor_info.scan_first; |
| 352 |
m_lastRange = m_sensor_info.scan_last; |
| 353 |
|
| 354 |
// Artificially reduced FOV? |
| 355 |
if (m_reduced_fov > 0 && m_reduced_fov < 2 * M_PI) |
| 356 |
{ |
| 357 |
int center = (m_lastRange + m_firstRange) >> 1; |
| 358 |
const int half_range = |
| 359 |
static_cast<int>((m_sensor_info.scans_per_360deg / 360.0) * RAD2DEG(m_reduced_fov)) >> 1; |
| 360 |
m_firstRange = center - half_range; |
| 361 |
m_lastRange = center + half_range; |
| 362 |
MRPT_LOG_INFO_STREAM( |
| 363 |
"[HOKUYO::turnOn] Using reduced FOV: ranges [" |
| 364 |
<< m_firstRange << "-" << m_lastRange << "] for " << RAD2DEG(m_reduced_fov) << " deg. FOV"); |
| 365 |
} |
| 366 |
|
| 367 |
if (!displayVersionInfo()) |
| 368 |
{ |
| 369 |
// return false; // It's not SO important |
| 370 |
} |
| 371 |
|
| 372 |
// Start! |
| 373 |
if (!startScanningMode()) return false; |
| 374 |
|
| 375 |
return true; |
| 376 |
|
| 377 |
MRPT_END |
| 378 |
} |
| 379 |
|
| 380 |
/*------------------------------------------------------------- |
| 381 |
turnOff |
| 382 |
-------------------------------------------------------------*/ |
| 383 |
bool CHokuyoURG::turnOff() |
| 384 |
{ |
| 385 |
// Turn off the laser: |
| 386 |
if (!switchLaserOff()) return false; |
| 387 |
|
| 388 |
return true; |
| 389 |
} |
| 390 |
|
| 391 |
bool CHokuyoURG::setHighBaudrate() |
| 392 |
{ |
| 393 |
if (!ensureStreamIsOpen()) return false; |
| 394 |
|
| 395 |
MRPT_LOG_DEBUG("[CHokuyoURG::setHighBaudrate] Changing baudrate to 115200..."); |
| 396 |
|
| 397 |
// Send command: |
| 398 |
sendCmd("SS115200\x0A"); |
| 399 |
|
| 400 |
// Receive response: |
| 401 |
if (!parseResponse()) |
| 402 |
{ |
| 403 |
MRPT_LOG_ERROR("[CHokuyoURG::setHighBaudrate] Error waiting for response"); |
| 404 |
return false; |
| 405 |
} |
| 406 |
|
| 407 |
MRPT_LOG_DEBUG("OK\n"); |
| 408 |
return true; |
| 409 |
} |
| 410 |
|
| 411 |
/*------------------------------------------------------------- |
| 412 |
ensureBufferHasBytes |
| 413 |
-------------------------------------------------------------*/ |
| 414 |
bool CHokuyoURG::ensureBufferHasBytes(size_t nDesiredBytes, bool additionalWaitForData) |
| 415 |
{ |
| 416 |
ASSERT_LT_(nDesiredBytes, m_rx_buffer.capacity()); |
| 417 |
|
| 418 |
if (m_rx_buffer.size() >= nDesiredBytes) return true; |
| 419 |
|
| 420 |
// Try to read more bytes: |
| 421 |
std::array<uint8_t, 512> buf; |
| 422 |
const size_t to_read = std::min(m_rx_buffer.available(), buf.size()); |
| 423 |
|
| 424 |
try |
| 425 |
{ |
| 426 |
const size_t nRead = [this, &buf, additionalWaitForData, to_read]() |
| 427 |
{ |
| 428 |
if (auto sock = dynamic_cast<CClientTCPSocket*>(m_stream.get()); sock) |
| 429 |
{ |
| 430 |
// socket: |
| 431 |
auto timeout = m_comms_timeout_ms; |
| 432 |
if (additionalWaitForData) mrpt::keep_max(timeout, 100); |
| 433 |
|
| 434 |
return sock->readAsync(&buf[0], to_read, timeout, m_comms_between_timeout_ms); |
| 435 |
} |
| 436 |
else |
| 437 |
{ |
| 438 |
// serial port: |
| 439 |
return m_stream->Read(&buf[0], to_read); |
| 440 |
} |
| 441 |
}(); |
| 442 |
|
| 443 |
m_rx_buffer.push_many(&buf[0], nRead); |
| 444 |
} |
| 445 |
catch (std::exception&) |
| 446 |
{ |
| 447 |
// 0 bytes read |
| 448 |
} |
| 449 |
|
| 450 |
return (m_rx_buffer.size() >= nDesiredBytes); |
| 451 |
} |
| 452 |
|
| 453 |
bool CHokuyoURG::parseResponse(bool additionalWaitForData) |
| 454 |
{ |
| 455 |
m_rcv_status0 = '\0'; |
| 456 |
m_rcv_status1 = '\0'; |
| 457 |
m_rcv_data.clear(); |
| 458 |
|
| 459 |
if (!ensureStreamIsOpen()) return false; |
| 460 |
ASSERT_(!m_lastSentMeasCmd.empty()); |
| 461 |
|
| 462 |
try |
| 463 |
{ |
| 464 |
// Process response: |
| 465 |
// --------------------------------- |
| 466 |
char tmp_rcv_status0 = '\0', tmp_rcv_status1 = '\0'; |
| 467 |
|
| 468 |
// COMMAND ECHO --------- |
| 469 |
size_t peekIdx = 0; |
| 470 |
const unsigned int verifLen = m_lastSentMeasCmd.size(); |
| 471 |
|
| 472 |
if (verifLen) |
| 473 |
{ |
| 474 |
unsigned int i = 0; |
| 475 |
do |
| 476 |
{ |
| 477 |
if (!ensureBufferHasBytes(verifLen - i, additionalWaitForData)) return false; |
| 478 |
|
| 479 |
// If matches the echo, go on: |
| 480 |
if (m_rx_buffer.peek(peekIdx++) == m_lastSentMeasCmd[i]) |
| 481 |
{ |
| 482 |
// Match is ok: |
| 483 |
i++; |
| 484 |
} |
| 485 |
else |
| 486 |
{ |
| 487 |
// Skip one byte and keep trying to match: |
| 488 |
m_rx_buffer.pop(); |
| 489 |
i = 0; |
| 490 |
peekIdx = 0; |
| 491 |
} |
| 492 |
} while (i < verifLen); |
| 493 |
} |
| 494 |
|
| 495 |
// Now, the status bytes: |
| 496 |
if (!ensureBufferHasBytes(peekIdx + 2, additionalWaitForData)) return false; |
| 497 |
|
| 498 |
tmp_rcv_status0 = m_rx_buffer.peek(peekIdx++); |
| 499 |
tmp_rcv_status1 = m_rx_buffer.peek(peekIdx++); |
| 500 |
|
| 501 |
// In SCIP2.0, there is an additional sum char: |
| 502 |
if (tmp_rcv_status1 != 0x0A) |
| 503 |
{ |
| 504 |
// Yes, it is SCIP2.0 |
| 505 |
if (!ensureBufferHasBytes(peekIdx + 1, additionalWaitForData)) return false; |
| 506 |
// Ignore this byte: sumStatus |
| 507 |
peekIdx++; |
| 508 |
} |
| 509 |
else |
| 510 |
{ |
| 511 |
// Continue, it seems a SCIP1.1 response... |
| 512 |
} |
| 513 |
|
| 514 |
// After the status bytes, there must be a LF: |
| 515 |
if (!ensureBufferHasBytes(peekIdx + 1, additionalWaitForData)) return false; |
| 516 |
char nextChar = m_rx_buffer.peek(peekIdx++); |
| 517 |
if (nextChar != 0x0A) return false; |
| 518 |
|
| 519 |
// ----------------------------------------------------------------------------- |
| 520 |
// Now the data: |
| 521 |
// There's a problem here, we don't know in advance how many bytes to |
| 522 |
// read, |
| 523 |
// so rely on the serial port class implemented buffer and call many |
| 524 |
// times |
| 525 |
// the read method with only 1 byte each time: |
| 526 |
// ----------------------------------------------------------------------------- |
| 527 |
bool lastWasLF = false; |
| 528 |
std::string tmp_rx; |
| 529 |
for (;;) |
| 530 |
{ |
| 531 |
if (!ensureBufferHasBytes(peekIdx + 1, additionalWaitForData)) |
| 532 |
{ |
| 533 |
// Do not empty the queue, it seems we need to wait for more |
| 534 |
// data: |
| 535 |
return false; |
| 536 |
} |
| 537 |
tmp_rx.push_back(m_rx_buffer.peek(peekIdx++)); |
| 538 |
|
| 539 |
// No data? |
| 540 |
if (tmp_rx.size() == 1 && tmp_rx[0] == 0x0A) |
| 541 |
{ |
| 542 |
tmp_rx.clear(); |
| 543 |
m_rcv_status0 = tmp_rcv_status0; |
| 544 |
m_rcv_status1 = tmp_rcv_status1; |
| 545 |
// Empty read bytes so far: |
| 546 |
for (size_t k = 0; k < peekIdx; k++) m_rx_buffer.pop(); |
| 547 |
return true; |
| 548 |
} |
| 549 |
|
| 550 |
// Is it a LF? |
| 551 |
if (*tmp_rx.rbegin() != 0x0A) |
| 552 |
{ |
| 553 |
lastWasLF = false; |
| 554 |
continue; |
| 555 |
} |
| 556 |
|
| 557 |
// This was a single LF: |
| 558 |
if (!lastWasLF) |
| 559 |
{ |
| 560 |
// Discard SUM+LF |
| 561 |
ASSERT_(tmp_rx.size() >= 2); |
| 562 |
tmp_rx.resize(tmp_rx.size() - 2); |
| 563 |
} |
| 564 |
else |
| 565 |
{ |
| 566 |
// This was a double LF. |
| 567 |
|
| 568 |
// Discard this last LF: |
| 569 |
tmp_rx.resize(tmp_rx.size() - 1); |
| 570 |
|
| 571 |
// Done! |
| 572 |
m_rcv_data = tmp_rx; |
| 573 |
m_rcv_status0 = tmp_rcv_status0; |
| 574 |
m_rcv_status1 = tmp_rcv_status1; |
| 575 |
|
| 576 |
// Empty read bytes so far: |
| 577 |
for (size_t k = 0; k < peekIdx; k++) m_rx_buffer.pop(); |
| 578 |
|
| 579 |
MRPT_LOG_DEBUG_STREAM("[Hokuyo] parseResponse(): RX `" << m_rcv_data << "`"); |
| 580 |
|
| 581 |
if (m_rcv_status0 != '0' && (m_rcv_status0 != '9' && m_rcv_status1 != '9')) |
| 582 |
{ |
| 583 |
MRPT_LOG_ERROR_STREAM( |
| 584 |
"[Hokuyo] Error LIDAR status: " << (int)m_rcv_status0 << " after command: `" |
| 585 |
<< m_lastSentMeasCmd << "`"); |
| 586 |
return false; |
| 587 |
} |
| 588 |
return true; |
| 589 |
} |
| 590 |
lastWasLF = true; |
| 591 |
} |
| 592 |
} |
| 593 |
catch (const std::exception& e) |
| 594 |
{ |
| 595 |
MRPT_LOG_ERROR_FMT("[Hokuyo] parseResponse() Exception: %s", e.what()); |
| 596 |
return false; |
| 597 |
} |
| 598 |
catch (...) |
| 599 |
{ |
| 600 |
return false; // Serial port timeout,... |
| 601 |
} |
| 602 |
} |
| 603 |
|
| 604 |
bool CHokuyoURG::enableSCIP20() |
| 605 |
{ |
| 606 |
if (!ensureStreamIsOpen()) return false; |
| 607 |
|
| 608 |
MRPT_LOG_DEBUG("[CHokuyoURG::enableSCIP20] Changing protocol to SCIP2.0..."); |
| 609 |
|
| 610 |
// Send command: |
| 611 |
sendCmd("SCIP2.0\x0A"); |
| 612 |
|
| 613 |
// Receive response: |
| 614 |
if (!parseResponse()) |
| 615 |
{ |
| 616 |
MRPT_LOG_ERROR_STREAM(__CURRENT_FUNCTION_NAME__ << ": Error in response"); |
| 617 |
return false; |
| 618 |
} |
| 619 |
|
| 620 |
MRPT_LOG_DEBUG("OK\n"); |
| 621 |
return true; |
| 622 |
} |
| 623 |
|
| 624 |
bool CHokuyoURG::switchLaserOn() |
| 625 |
{ |
| 626 |
if (!ensureStreamIsOpen()) return false; |
| 627 |
|
| 628 |
MRPT_LOG_DEBUG("[CHokuyoURG::switchLaserOn] Switching laser ON..."); |
| 629 |
|
| 630 |
// Send command: |
| 631 |
sendCmd("BM\x0A"); |
| 632 |
|
| 633 |
// Receive response: |
| 634 |
if (!parseResponse()) |
| 635 |
{ |
| 636 |
MRPT_LOG_ERROR_STREAM(__CURRENT_FUNCTION_NAME__ << ": Error in response"); |
| 637 |
return false; |
| 638 |
} |
| 639 |
|
| 640 |
MRPT_LOG_DEBUG("OK\n"); |
| 641 |
return true; |
| 642 |
} |
| 643 |
|
| 644 |
bool CHokuyoURG::switchLaserOff() |
| 645 |
{ |
| 646 |
if (!ensureStreamIsOpen()) return false; |
| 647 |
|
| 648 |
MRPT_LOG_DEBUG("[CHokuyoURG::switchLaserOff] Switching laser OFF..."); |
| 649 |
|
| 650 |
// Send command: |
| 651 |
sendCmd("QT\x0A"); |
| 652 |
|
| 653 |
// Receive response: |
| 654 |
if (!parseResponse()) |
| 655 |
{ |
| 656 |
MRPT_LOG_ERROR_STREAM(__CURRENT_FUNCTION_NAME__ << ": Error in response"); |
| 657 |
return false; |
| 658 |
} |
| 659 |
|
| 660 |
MRPT_LOG_DEBUG("OK\n"); |
| 661 |
return true; |
| 662 |
} |
| 663 |
|
| 664 |
void CHokuyoURG::setScanInterval(unsigned int skipScanCount) { m_scan_interval = skipScanCount; } |
| 665 |
unsigned int CHokuyoURG::getScanInterval() const { return m_scan_interval; } |
| 666 |
bool CHokuyoURG::setMotorSpeed(int motoSpeed_rpm) |
| 667 |
{ |
| 668 |
if (!ensureStreamIsOpen()) return false; |
| 669 |
|
| 670 |
MRPT_LOG_DEBUG_FMT("[CHokuyoURG::setMotorSpeed] Setting to %i rpm...", motoSpeed_rpm); |
| 671 |
|
| 672 |
// Send command: |
| 673 |
int motorSpeedCode = (600 - motoSpeed_rpm) / 6; |
| 674 |
if (motorSpeedCode < 0 || motorSpeedCode > 10) |
| 675 |
{ |
| 676 |
MRPT_LOG_ERROR_STREAM( |
| 677 |
__CURRENT_FUNCTION_NAME__ << " Motorspeed must be in the range 540-600 rpm"); |
| 678 |
return false; |
| 679 |
} |
| 680 |
|
| 681 |
char cmd[20]; |
| 682 |
os::sprintf(cmd, 20, "CR%02i\x0A", motorSpeedCode); |
| 683 |
sendCmd(cmd); |
| 684 |
|
| 685 |
// Receive response: |
| 686 |
if (!parseResponse()) |
| 687 |
{ |
| 688 |
MRPT_LOG_ERROR_STREAM(__CURRENT_FUNCTION_NAME__ << ": Error in response"); |
| 689 |
return false; |
| 690 |
} |
| 691 |
|
| 692 |
MRPT_LOG_DEBUG("OK\n"); |
| 693 |
return true; |
| 694 |
} |
| 695 |
|
| 696 |
/*------------------------------------------------------------- |
| 697 |
setHighSensitivityMode |
| 698 |
-------------------------------------------------------------*/ |
| 699 |
bool CHokuyoURG::setHighSensitivityMode(bool enabled) |
| 700 |
{ |
| 701 |
if (!ensureStreamIsOpen()) return false; |
| 702 |
|
| 703 |
MRPT_LOG_DEBUG_FMT( |
| 704 |
"[CHokuyoURG::setHighSensitivityMode] Setting HS mode to: %s...", enabled ? "true" : "false"); |
| 705 |
|
| 706 |
// Send command: |
| 707 |
char cmd[20]; |
| 708 |
os::sprintf(cmd, 20, "HS%i\x0A", enabled ? 1 : 0); |
| 709 |
sendCmd(cmd); |
| 710 |
|
| 711 |
// Receive response: |
| 712 |
if (!parseResponse()) |
| 713 |
{ |
| 714 |
MRPT_LOG_ERROR_STREAM(__CURRENT_FUNCTION_NAME__ << ": Error in response"); |
| 715 |
return false; |
| 716 |
} |
| 717 |
|
| 718 |
MRPT_LOG_DEBUG("OK\n"); |
| 719 |
return true; |
| 720 |
} |
| 721 |
|
| 722 |
/*------------------------------------------------------------- |
| 723 |
setIntensityMode |
| 724 |
-------------------------------------------------------------*/ |
| 725 |
bool CHokuyoURG::setIntensityMode(bool enabled) |
| 726 |
{ |
| 727 |
m_intensity = enabled; |
| 728 |
return true; |
| 729 |
} |
| 730 |
|
| 731 |
bool CHokuyoURG::displayVersionInfo() |
| 732 |
{ |
| 733 |
if (!ensureStreamIsOpen()) return false; |
| 734 |
|
| 735 |
MRPT_LOG_DEBUG("[CHokuyoURG::displayVersionInfo] Asking info..."); |
| 736 |
|
| 737 |
// Send command: |
| 738 |
sendCmd("VV\x0A"); |
| 739 |
|
| 740 |
// Receive response: |
| 741 |
if (!parseResponse()) |
| 742 |
{ |
| 743 |
MRPT_LOG_ERROR_STREAM(__CURRENT_FUNCTION_NAME__ << ": Error in response"); |
| 744 |
return false; |
| 745 |
} |
| 746 |
|
| 747 |
MRPT_LOG_DEBUG("OK\n"); |
| 748 |
|
| 749 |
// PRINT: |
| 750 |
for (auto& c : m_rcv_data) |
| 751 |
if (c == ';') c = '\n'; |
| 752 |
m_rcv_data[m_rcv_data.size()] = '\0'; |
| 753 |
|
| 754 |
if (!m_rcv_data.empty()) |
| 755 |
{ |
| 756 |
MRPT_LOG_INFO_STREAM( |
| 757 |
"\n------------- HOKUYO Scanner: Version Information ------\n" |
| 758 |
<< &m_rcv_data[0] |
| 759 |
<< "\n" |
| 760 |
"-------------------------------------------------------\n\n"); |
| 761 |
} |
| 762 |
return true; |
| 763 |
} |
| 764 |
|
| 765 |
/*------------------------------------------------------------- |
| 766 |
displaySensorInfo |
| 767 |
-------------------------------------------------------------*/ |
| 768 |
bool CHokuyoURG::displaySensorInfo(TSensorInfo* out_data) |
| 769 |
{ |
| 770 |
if (!ensureStreamIsOpen()) return false; |
| 771 |
|
| 772 |
MRPT_LOG_DEBUG("[CHokuyoURG::displaySensorInfo] Asking for info..."); |
| 773 |
|
| 774 |
// Send command: |
| 775 |
sendCmd("PP\x0A"); |
| 776 |
|
| 777 |
// Receive response: |
| 778 |
if (!parseResponse()) |
| 779 |
{ |
| 780 |
MRPT_LOG_ERROR_STREAM(__CURRENT_FUNCTION_NAME__ << ": Error in response"); |
| 781 |
return false; |
| 782 |
} |
| 783 |
MRPT_LOG_DEBUG("OK\n"); |
| 784 |
|
| 785 |
// PRINT: |
| 786 |
for (auto& c : m_rcv_data) |
| 787 |
if (c == ';') c = '\n'; |
| 788 |
m_rcv_data[m_rcv_data.size()] = '\0'; |
| 789 |
|
| 790 |
if (!m_rcv_data.empty()) |
| 791 |
{ |
| 792 |
MRPT_LOG_INFO_STREAM( |
| 793 |
"\n------------- HOKUYO Scanner: Product Information ------\n" |
| 794 |
<< &m_rcv_data[0] |
| 795 |
<< "\n" |
| 796 |
"-------------------------------------------------------\n\n"); |
| 797 |
} |
| 798 |
|
| 799 |
// Parse the data: |
| 800 |
if (out_data) |
| 801 |
{ |
| 802 |
const char* ptr; |
| 803 |
|
| 804 |
if (nullptr != (ptr = strstr(&m_rcv_data[0], "DMAX:"))) |
| 805 |
out_data->d_max = 0.001 * atoi(ptr + 5); |
| 806 |
else |
| 807 |
MRPT_LOG_ERROR("[Hokuyo] displayVersionInfo() parse error"); |
| 808 |
|
| 809 |
if (nullptr != (ptr = strstr(&m_rcv_data[0], "DMIN:"))) |
| 810 |
out_data->d_min = 0.001 * atoi(ptr + 5); |
| 811 |
else |
| 812 |
MRPT_LOG_ERROR("[Hokuyo] displayVersionInfo() parse error"); |
| 813 |
|
| 814 |
if (nullptr != (ptr = strstr(&m_rcv_data[0], "ARES:"))) |
| 815 |
out_data->scans_per_360deg = atoi(ptr + 5); |
| 816 |
else |
| 817 |
MRPT_LOG_ERROR("[Hokuyo] displayVersionInfo() parse error"); |
| 818 |
|
| 819 |
if (nullptr != (ptr = strstr(&m_rcv_data[0], "SCAN:"))) |
| 820 |
out_data->motor_speed_rpm = atoi(ptr + 5); |
| 821 |
else |
| 822 |
MRPT_LOG_ERROR("[Hokuyo] displayVersionInfo() parse error"); |
| 823 |
|
| 824 |
if (nullptr != (ptr = strstr(&m_rcv_data[0], "AMIN:"))) |
| 825 |
out_data->scan_first = atoi(ptr + 5); |
| 826 |
else |
| 827 |
MRPT_LOG_ERROR("[Hokuyo] displayVersionInfo() parse error"); |
| 828 |
|
| 829 |
if (nullptr != (ptr = strstr(&m_rcv_data[0], "AMAX:"))) |
| 830 |
out_data->scan_last = atoi(ptr + 5); |
| 831 |
else |
| 832 |
MRPT_LOG_ERROR("[Hokuyo] displayVersionInfo() parse error"); |
| 833 |
|
| 834 |
if (nullptr != (ptr = strstr(&m_rcv_data[0], "AFRT:"))) |
| 835 |
out_data->scan_front = atoi(ptr + 5); |
| 836 |
else |
| 837 |
MRPT_LOG_ERROR("[Hokuyo] displayVersionInfo() parse error"); |
| 838 |
|
| 839 |
if (nullptr != (ptr = strstr(&m_rcv_data[0], "MODL:"))) |
| 840 |
{ |
| 841 |
char aux[30]; |
| 842 |
memcpy(aux, ptr + 5, 8); |
| 843 |
aux[8] = '\0'; |
| 844 |
out_data->model = aux; |
| 845 |
} |
| 846 |
else |
| 847 |
MRPT_LOG_ERROR("[Hokuyo] displayVersionInfo() parse error"); |
| 848 |
} |
| 849 |
|
| 850 |
return true; |
| 851 |
} |
| 852 |
|
| 853 |
bool CHokuyoURG::startScanningMode() |
| 854 |
{ |
| 855 |
if (!ensureStreamIsOpen()) return false; |
| 856 |
|
| 857 |
MRPT_LOG_DEBUG("[CHokuyoURG::startScanningMode] Starting scanning mode..."); |
| 858 |
|
| 859 |
// Send command: |
| 860 |
// 'M' 'D' |
| 861 |
// 'XXXX' (starting step) |
| 862 |
// 'XXXX' (end step) |
| 863 |
// 'XX' (cluster count) |
| 864 |
// 'X' (scan interval) |
| 865 |
// 'XX' (number of scans) |
| 866 |
char cmd[50]; |
| 867 |
unsigned int scan_interval = m_scan_interval; |
| 868 |
if (scan_interval > 9) scan_interval = 9; |
| 869 |
os::sprintf( |
| 870 |
cmd, 50, "M%c%04u%04u01%u00\x0A", m_intensity ? 'E' : 'D', m_firstRange, m_lastRange, |
| 871 |
scan_interval); |
| 872 |
|
| 873 |
sendCmd(cmd); |
| 874 |
|
| 875 |
// Receive response: |
| 876 |
if (!parseResponse()) |
| 877 |
{ |
| 878 |
MRPT_LOG_ERROR_STREAM(__CURRENT_FUNCTION_NAME__ << ": Error in response"); |
| 879 |
return false; |
| 880 |
} |
| 881 |
|
| 882 |
MRPT_LOG_DEBUG("OK\n"); |
| 883 |
return true; |
| 884 |
} |
| 885 |
|
| 886 |
bool CHokuyoURG::ensureStreamIsOpen() |
| 887 |
{ |
| 888 |
MRPT_START |
| 889 |
|
| 890 |
if (m_stream) |
| 891 |
{ |
| 892 |
// Socket or USB connection? |
| 893 |
if (!m_ip_dir.empty() && m_port_dir) |
| 894 |
{ |
| 895 |
// Has the port been disconected (USB serial ports)?? |
| 896 |
auto* COM = dynamic_cast<CClientTCPSocket*>(m_stream.get()); |
| 897 |
|
| 898 |
if (COM != nullptr) |
| 899 |
{ |
| 900 |
if (COM->isConnected()) return true; |
| 901 |
|
| 902 |
// It has been disconnected... try to reconnect: |
| 903 |
MRPT_LOG_ERROR( |
| 904 |
"[CHokuyoURG] Socket connection lost! trying to " |
| 905 |
"reconnect..."); |
| 906 |
|
| 907 |
try |
| 908 |
{ |
| 909 |
COM->connect(m_ip_dir, m_port_dir); |
| 910 |
// OK, reconfigure the laser: |
| 911 |
turnOn(); |
| 912 |
return true; |
| 913 |
} |
| 914 |
catch (...) |
| 915 |
{ |
| 916 |
// Not yet.. |
| 917 |
return false; |
| 918 |
} |
| 919 |
} |
| 920 |
else |
| 921 |
{ |
| 922 |
return true; // Assume OK |
| 923 |
} |
| 924 |
} |
| 925 |
else |
| 926 |
{ |
| 927 |
// Has the port been disconected (USB serial ports)?? |
| 928 |
auto* COM = dynamic_cast<CSerialPort*>(m_stream.get()); |
| 929 |
if (COM != nullptr) |
| 930 |
{ |
| 931 |
if (COM->isOpen()) return true; |
| 932 |
|
| 933 |
// It has been disconnected... try to reconnect: |
| 934 |
MRPT_LOG_ERROR_STREAM( |
| 935 |
__CURRENT_FUNCTION_NAME__ << ": Serial port connection lost! Trying to reconnect..."); |
| 936 |
|
| 937 |
try |
| 938 |
{ |
| 939 |
COM->open(); |
| 940 |
// OK, reconfigure the laser: |
| 941 |
turnOn(); |
| 942 |
return true; |
| 943 |
} |
| 944 |
catch (...) |
| 945 |
{ |
| 946 |
// Not yet.. |
| 947 |
return false; |
| 948 |
} |
| 949 |
} |
| 950 |
else |
| 951 |
{ |
| 952 |
return true; // Assume OK |
| 953 |
} |
| 954 |
} |
| 955 |
} |
| 956 |
else |
| 957 |
{ |
| 958 |
if (m_com_port.empty() && (m_ip_dir.empty() || !m_port_dir)) |
| 959 |
{ |
| 960 |
THROW_EXCEPTION( |
| 961 |
"No stream bound to the laser nor COM serial port or ip and " |
| 962 |
"port provided in 'm_com_port','m_ip_dir' and 'm_port_dir'"); |
| 963 |
} |
| 964 |
|
| 965 |
if (!m_ip_dir.empty()) |
| 966 |
{ |
| 967 |
// Connect to the TCP/IP port: |
| 968 |
auto theCOM = std::make_shared<CClientTCPSocket>(); |
| 969 |
|
| 970 |
MRPT_LOG_INFO_STREAM( |
| 971 |
__CURRENT_FUNCTION_NAME__ << " Connecting to " << m_ip_dir << ":" << m_port_dir); |
| 972 |
theCOM->connect(m_ip_dir, m_port_dir); |
| 973 |
|
| 974 |
if (!theCOM->isConnected()) |
| 975 |
{ |
| 976 |
MRPT_LOG_ERROR_STREAM( |
| 977 |
__CURRENT_FUNCTION_NAME__ << " Cannot connect with the server '" << m_com_port << "'"); |
| 978 |
return false; |
| 979 |
} |
| 980 |
|
| 981 |
// Bind: |
| 982 |
bindIO(theCOM); |
| 983 |
} |
| 984 |
else |
| 985 |
{ |
| 986 |
// Try to open the serial port: |
| 987 |
auto theCOM = std::make_shared<CSerialPort>(m_com_port, true); |
| 988 |
|
| 989 |
if (!theCOM->isOpen()) |
| 990 |
{ |
| 991 |
MRPT_LOG_ERROR_STREAM( |
| 992 |
__CURRENT_FUNCTION_NAME__ << " Cannot open serial port '" << m_com_port << "'"); |
| 993 |
return false; |
| 994 |
} |
| 995 |
|
| 996 |
// Bind: |
| 997 |
bindIO(theCOM); |
| 998 |
} |
| 999 |
|
| 1000 |
// (re)connected to the sensor. Configure the laser: |
| 1001 |
turnOn(); |
| 1002 |
|
| 1003 |
return true; |
| 1004 |
} |
| 1005 |
MRPT_END |
| 1006 |
} |
| 1007 |
|
| 1008 |
void CHokuyoURG::initialize() |
| 1009 |
{ |
| 1010 |
if (m_verbose) this->setMinLoggingLevel(mrpt::system::LVL_DEBUG); |
| 1011 |
|
| 1012 |
if (!ensureStreamIsOpen() || !turnOn()) |
| 1013 |
{ |
| 1014 |
m_state = ssError; |
| 1015 |
|
| 1016 |
MRPT_LOG_ERROR("[Hokuyo] Error initializing HOKUYO scanner"); |
| 1017 |
return; |
| 1018 |
} |
| 1019 |
} |
| 1020 |
|
| 1021 |
void CHokuyoURG::purgeBuffers() |
| 1022 |
{ |
| 1023 |
if (!ensureStreamIsOpen()) return; |
| 1024 |
|
| 1025 |
if (m_ip_dir.empty()) |
| 1026 |
{ |
| 1027 |
auto* COM = dynamic_cast<CSerialPort*>(m_stream.get()); |
| 1028 |
if (COM != nullptr) |
| 1029 |
{ |
| 1030 |
COM->purgeBuffers(); |
| 1031 |
} |
| 1032 |
} |
| 1033 |
else // Socket connection |
| 1034 |
{ |
| 1035 |
auto* COM = dynamic_cast<CClientTCPSocket*>(m_stream.get()); |
| 1036 |
|
| 1037 |
size_t to_read = COM->getReadPendingBytes(); |
| 1038 |
|
| 1039 |
if (to_read) |
| 1040 |
{ |
| 1041 |
void* buf = malloc(sizeof(uint8_t) * to_read); |
| 1042 |
|
| 1043 |
size_t nRead = m_stream->Read(buf, to_read); |
| 1044 |
|
| 1045 |
if (nRead != to_read) |
| 1046 |
THROW_EXCEPTION( |
| 1047 |
"Error in purge buffers: read and expected number of bytes " |
| 1048 |
"are different."); |
| 1049 |
|
| 1050 |
free(buf); |
| 1051 |
} |
| 1052 |
} |
| 1053 |
} |