Go to the documentation of this file.
30 CHokuyoURG::CHokuyoURG() : m_rx_buffer(40000) {
m_sensorLabel =
"Hokuyo"; }
39 catch (
const std::exception& e)
59 const size_t N = strlen(str);
60 const size_t nWriten =
m_stream->Write(str, N);
70 bool& outThereIsObservation,
73 outThereIsObservation =
false;
74 hardwareError =
false;
88 int expectedSize = nRanges * 3 + 4;
102 hardwareError =
true;
119 hardwareError =
true;
128 if ((
size_t)expectedSize !=
m_rcv_data.size())
131 "[CHokuyoURG::doProcess] ERROR: Expected "
132 << expectedSize <<
" data bytes, received " <<
m_rcv_data.size()
134 hardwareError =
true;
140 if (do_timestamp_sync &&
143 do_timestamp_sync =
false;
147 if (do_timestamp_sync)
150 uint32_t nowUI = ((
m_rcv_data[0] - 0x30) << 18) +
163 auto AtDO = std::chrono::milliseconds(AtUI);
183 for (
int i = 0; i < nRanges; i++)
185 int b1 = (*ptr++) - 0x30;
186 int b2 = (*ptr++) - 0x30;
187 int b3 = (*ptr++) - 0x30;
189 int range_mm = ((
b1 << 12) | (
b2 << 6) |
b3);
198 int b4 = (*ptr++) - 0x30;
199 int b5 = (*ptr++) - 0x30;
200 int b6 = (*ptr++) - 0x30;
211 outThereIsObservation =
true;
220 const std::string& iniSection)
226 configSource.
read_int(iniSection,
"HOKUYO_motorSpeed_rpm", 0);
228 configSource.
read_float(iniSection,
"pose_x", 0),
229 configSource.
read_float(iniSection,
"pose_y", 0),
230 configSource.
read_float(iniSection,
"pose_z", 0),
251 "Either COM_port or IP_DIR must be defined in the configuration file!");
254 "Both COM_port and IP_DIR set! Please, define only one of them.");
259 "A TCP/IP port number `PORT_DIR` must be specified for Ethernet "
293 COM->setTimeouts(100, 0, 200, 0, 0);
297 std::this_thread::sleep_for(10ms);
300 std::this_thread::sleep_for(10ms);
302 COM->setConfig(115200);
304 std::this_thread::sleep_for(10ms);
306 std::this_thread::sleep_for(10ms);
307 COM->setConfig(19200);
315 COM->setConfig(115200);
326 std::this_thread::sleep_for(10ms);
329 std::this_thread::sleep_for(10ms);
332 std::this_thread::sleep_for(10ms);
360 const int half_range =
static_cast<int>(
367 "[HOKUYO::turnOn] Using reduced FOV: ranges ["
401 "[CHokuyoURG::setHighBaudrate] Changing baudrate to 115200...");
410 "[CHokuyoURG::setHighBaudrate] Error waiting for response");
428 std::array<uint8_t, 128> buf;
434 const size_t nRead = sock ? sock->
readAsync(&buf[0], to_read, 100, 10)
438 catch (std::exception&)
459 char tmp_rcv_status0 =
'\0', tmp_rcv_status1 =
'\0';
485 }
while (i < verifLen);
495 if (tmp_rcv_status1 != 0x0A)
510 if (nextChar != 0x0A)
return false;
520 bool lastWasLF =
false;
533 if (tmp_rx.size() == 1 && tmp_rx[0] == 0x0A)
544 if (*tmp_rx.rbegin() != 0x0A)
555 tmp_rx.resize(tmp_rx.size() - 2);
562 tmp_rx.resize(tmp_rx.size() - 1);
573 "[Hokuyo] parseResponse(): RX `" <<
m_rcv_data <<
"`");
579 "[Hokuyo] Error LIDAR status: "
589 catch (
const std::exception& e)
605 "[CHokuyoURG::enableSCIP20] Changing protocol to SCIP2.0...");
626 MRPT_LOG_DEBUG(
"[CHokuyoURG::switchLaserOn] Switching laser ON...");
647 MRPT_LOG_DEBUG(
"[CHokuyoURG::switchLaserOff] Switching laser OFF...");
674 "[CHokuyoURG::setMotorSpeed] Setting to %i rpm...", motoSpeed_rpm);
677 int motorSpeedCode = (600 - motoSpeed_rpm) / 6;
678 if (motorSpeedCode < 0 || motorSpeedCode > 10)
682 <<
" Motorspeed must be in the range 540-600 rpm");
687 os::sprintf(cmd, 20,
"CR%02i\x0A", motorSpeedCode);
710 "[CHokuyoURG::setHighSensitivityMode] Setting HS mode to: %s...",
711 enabled ?
"true" :
"false");
743 MRPT_LOG_DEBUG(
"[CHokuyoURG::displayVersionInfo] Asking info...");
760 if (c ==
';') c =
'\n';
766 "\n------------- HOKUYO Scanner: Version Information ------\n"
769 "-------------------------------------------------------\n\n");
781 MRPT_LOG_DEBUG(
"[CHokuyoURG::displaySensorInfo] Asking for info...");
797 if (c ==
';') c =
'\n';
803 "\n------------- HOKUYO Scanner: Product Information ------\n"
806 "-------------------------------------------------------\n\n");
814 if (
nullptr != (ptr = strstr(&
m_rcv_data[0],
"DMAX:")))
815 out_data->
d_max = 0.001 * atoi(ptr + 5);
819 if (
nullptr != (ptr = strstr(&
m_rcv_data[0],
"DMIN:")))
820 out_data->
d_min = 0.001 * atoi(ptr + 5);
824 if (
nullptr != (ptr = strstr(&
m_rcv_data[0],
"ARES:")))
829 if (
nullptr != (ptr = strstr(&
m_rcv_data[0],
"SCAN:")))
834 if (
nullptr != (ptr = strstr(&
m_rcv_data[0],
"AMIN:")))
839 if (
nullptr != (ptr = strstr(&
m_rcv_data[0],
"AMAX:")))
844 if (
nullptr != (ptr = strstr(&
m_rcv_data[0],
"AFRT:")))
849 if (
nullptr != (ptr = strstr(&
m_rcv_data[0],
"MODL:")))
854 out_data->
model = aux;
867 MRPT_LOG_DEBUG(
"[CHokuyoURG::startScanningMode] Starting scanning mode...");
878 if (scan_interval > 9) scan_interval = 9;
911 if (COM->isConnected())
return true;
915 "[CHokuyoURG] Socket connection lost! trying to "
942 if (COM->isOpen())
return true;
947 <<
": Serial port connection lost! Trying to reconnect...");
973 "No stream bound to the laser nor COM serial port or ip and "
974 "port provided in 'm_com_port','m_ip_dir' and 'm_port_dir'");
980 auto theCOM = std::make_shared<CClientTCPSocket>();
987 if (!theCOM->isConnected())
991 <<
" Cannot connect with the server '" <<
m_com_port
1002 auto theCOM = std::make_shared<CSerialPort>(
m_com_port,
true);
1004 if (!theCOM->isOpen())
1057 void* buf = malloc(
sizeof(uint8_t) * to_read);
1059 size_t nRead =
m_stream->Read(buf, to_read);
1061 if (nRead != to_read)
1063 "Error in purge buffers: read and expected number of bytes "
size_t getReadPendingBytes()
Return the number of bytes already in the receive queue (they can be read without waiting)
A TCP socket that can be connected to a TCP server, implementing MRPT's CStream interface for passing...
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.
mrpt::containers::circular_buffer< uint8_t > m_rx_buffer
Auxiliary buffer for readings.
#define ASSERT_EQUAL_(__A, __B)
Assert comparing two values, reporting their actual values upon failure.
#define __CURRENT_FUNCTION_NAME__
A macro for obtaining the name of the current function:
bool internal_notifyNoScanReceived()
Must be called from doProcessSimple() implementations.
#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...
void bindIO(const std::shared_ptr< mrpt::io::CStream > &streamIO)
Binds the object to a given I/O channel.
const int MINIMUM_PACKETS_TO_SET_TIMESTAMP_REFERENCE
const float & getScanRange(const size_t i) const
The range values of the scan, in meters.
void setFromValues(const double x0, const double y0, const double z0, const double yaw=0, const double pitch=0, const double roll=0)
Set the pose from a 3D position (meters) and yaw/pitch/roll angles (radians) - This method recomputes...
uint32_t m_timeStartUI
Time of the first data packet, for synchronization purposes.
Used in CHokuyoURG::displayVersionInfo.
void closeStreamConnection()
Called upon dtor, or when trying to recover from a disconnected sensor.
void filterByExclusionAngles(mrpt::obs::CObservation2DRangeScan &obs) const
Mark as invalid those ranges in a set of forbiden angle ranges.
mrpt::system::TTimeStamp now()
A shortcut for system::getCurrentTime.
A communications serial port built as an implementation of a utils::CStream.
int m_timeStartSynchDelay
Counter to discard to first few packets before setting the correspondence between device and computer...
int m_firstRange
The first and last ranges to consider from the scan.
bool read_bool(const std::string §ion, const std::string &name, bool defaultValue, bool failIfNotFound=false) const
Contains classes for various device interfaces.
Serial and networking devices and utilities.
void internal_notifyGoodScanNow()
Must be called from doProcessSimple() implementations.
~CHokuyoURG() override
Destructor: turns the laser off.
bool enableSCIP20()
Enables the SCIP2.0 protocol (this must be called at the very begining!).
void resizeScan(const size_t len)
Resizes all data vectors to allocate a given number of scan rays.
bool turnOff() override
Disables the scanning mode (this can be used to turn the device in low energy mode,...
#define THROW_EXCEPTION(msg)
void purgeBuffers()
Purge tx and rx buffers.
#define ASSERT_(f)
Defines an assertion mechanism.
void setScanRangeValidity(const size_t i, const bool val)
double d_min
Min/Max ranges, in meters.
#define MRPT_LOG_DEBUG_FMT(_FMT_STRING,...)
Use: MRPT_LOG_DEBUG_FMT("i=%u", i);
bool ensureStreamIsOpen()
Returns true if there is a valid stream bound to the laser scanner, otherwise it first try to open th...
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
mrpt::system::TTimeStamp m_timeStartTT
int m_motorSpeed_rpm
The motor speed (default=600rpm)
bool turnOn() override
Enables the scanning mode (which may depend on the specific laser device); this must be called before...
int read_int(const std::string §ion, const std::string &name, int defaultValue, bool failIfNotFound=false) const
unsigned int getScanInterval() const
void setScanIntensity(const size_t i, const int val)
int sprintf(char *buf, size_t bufSize, const char *format,...) noexcept MRPT_printf_format_check(3
An OS-independent version of sprintf (Notice the bufSize param, which may be ignored in some compiler...
unsigned int m_port_dir
If set to non-empty and m_ip_dir too, the program will try to connect to a Hokuyo using Ethernet comm...
void setConfig(int baudRate, int parity=0, int bits=8, int nStopBits=1, bool enableFlowControl=false)
Changes the configuration of the port.
#define ASSERT_BELOW_(__A, __B)
bool m_disable_firmware_timestamp
bool switchLaserOn()
Switchs the laser on.
#define MRPT_LOAD_HERE_CONFIG_VAR( variableName, variableType, targetVariable, configFileObject, sectionNameStr)
size_t capacity() const
Return the maximum capacity of the buffer.
void purgeBuffers()
Empties the RX buffers of the serial port.
size_t size() const
Return the number of elements available for read ("pop") in the buffer (this is NOT the maximum size ...
float aperture
The "aperture" or field-of-view of the range finder, in radians (typically M_PI = 180 degrees).
void setScanHasIntensity(bool setHasIntensityFlag)
Marks this scan as having or not intensity data.
T peek() const
Peek (see without modifying) what is to be read from the buffer if pop() was to be called.
void loadConfig_sensorSpecific(const mrpt::config::CConfigFileBase &configSource, const std::string &iniSection) override
See the class documentation at the top for expected parameters.
This class allows loading and storing values and vectors of different types from a configuration text...
bool ensureBufferHasBytes(const size_t nDesiredBytes)
Assures a minimum number of bytes in the input buffer, reading from the serial port only if required.
void loadCommonParams(const mrpt::config::CConfigFileBase &configSource, const std::string &iniSection)
Should be call by derived classes at "loadConfig" (loads exclusion areas AND exclusion angles).
unsigned int m_scan_interval
constexpr double RAD2DEG(const double x)
Radians to degrees.
#define MRPT_LOG_ERROR_FMT(_FMT_STRING,...)
#define MRPT_LOG_ERROR(_STRING)
void doProcessSimple(bool &outThereIsObservation, mrpt::obs::CObservation2DRangeScan &outObservation, bool &hardwareError) override
Specific laser scanner "software drivers" must process here new data from the I/O stream,...
std::string m_lastSentMeasCmd
The last sent measurement command (MDXXX), including the last 0x0A.
mrpt::gui::CDisplayWindow3D::Ptr m_win
void initialize() override
Turns the laser on.
bool setIntensityMode(bool enabled)
If true scans will capture intensity.
void setMinLoggingLevel(const VerbosityLevel level)
Set the minimum logging level for which the incoming logs are going to be taken into account.
double m_reduced_fov
Used to reduce artificially the interval of scan ranges.
size_t readAsync(void *Buffer, const size_t Count, const int timeoutStart_ms=-1, const int timeoutBetween_ms=-1)
A method for reading from the socket with an optional timeout.
std::string m_sensorLabel
See CGenericSensor.
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
bool displayVersionInfo()
Ask to the device, and print to the debug stream, details about the firmware version,...
bool displaySensorInfo(CHokuyoURG::TSensorInfo *out_data=nullptr)
Ask to the device, and print to the debug stream, details about the sensor model.
T pop()
Retrieve an element from the buffer.
constexpr double DEG2RAD(const double x)
Degrees to radians
int scan_first
First, last, and front step of the scanner angular span.
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
void push_many(T *array_elements, size_t count)
Insert an array of elements in the buffer.
#define IMPLEMENTS_GENERIC_SENSOR(class_name, NameSpace)
This must be inserted in all CGenericSensor classes implementation files:
size_t available() const
The maximum number of elements that can be written ("push") without rising an overflow error.
void sendCmd(const char *str)
bool setHighSensitivityMode(bool enabled)
Changes the high sensitivity mode (HS) (default: false)
void processPreview(const mrpt::obs::CObservation2DRangeScan &obs)
Must be called inside the capture method to allow optional GUI preview of scans.
#define MRPT_LOG_DEBUG_STREAM(__CONTENTS)
Use: MRPT_LOG_DEBUG_STREAM("Var=" << value << " foo=" << foo_var);
#define MRPT_LOG_ERROR_STREAM(__CONTENTS)
std::shared_ptr< mrpt::io::CStream > m_stream
The I/O channel (will be nullptr if not bound).
std::string m_ip_dir
If set to non-empty and m_port_dir too, the program will try to connect to a Hokuyo using Ethernet co...
bool parseResponse()
Parses the response from the device from raw bytes in m_rx_buffer, and stored the received frame in m...
int scans_per_360deg
Number of measuremens per 360 degrees.
This software driver implements the protocol SCIP-2.0 for interfacing HOKUYO URG/UTM/UXM/UST laser sc...
std::string m_com_port
If set to non-empty, the serial port will be attempted to be opened automatically when this class is ...
int motor_speed_rpm
Standard motor speed, rpm.
#define MRPT_LOG_INFO_STREAM(__CONTENTS)
bool switchLaserOff()
Switchs the laser off.
std::string model
The sensor model.
std::string m_rcv_data
temp buffer for incoming data packets
std::string exception_to_str(const std::exception &e)
Builds a nice textual representation of a nested exception, which if generated using MRPT macros (THR...
bool setMotorSpeed(int motoSpeed_rpm)
Changes the motor speed in rpm's (default 600rpm)
mrpt::poses::CPose3D sensorPose
The 6D pose of the sensor on the robot at the moment of starting the scan.
bool startScanningMode()
Start the continuous scanning mode, using parameters stored in the object (loaded from the ....
void setScanInterval(unsigned int skipScanCount)
Set the skip scan count (0 means send all scans).
The namespace for 3D scene representation and rendering.
void filterByExclusionAreas(mrpt::obs::CObservation2DRangeScan &obs) const
Mark as invalid those points which (x,y) coordinates fall within the exclusion polygons.
TSensorInfo m_sensor_info
The information gathered when the laser is first open.
bool m_intensity
Get intensity from lidar scan (default: false)
bool setHighBaudrate()
Passes to 115200bps bitrate.
bool m_highSensMode
High sensitivity [HS] mode (default: false)
poses::CPose3D m_sensorPose
The sensor 6D pose:
void memcpy(void *dest, size_t destSize, const void *src, size_t copyCount) noexcept
An OS and compiler independent version of "memcpy".
Page generated by Doxygen 1.8.17 for MRPT 2.0.3 at Fri May 29 13:06:46 UTC 2020 | |