32 CHokuyoURG::CHokuyoURG()
33 : m_firstRange(44), m_lastRange(725), m_motorSpeed_rpm(0),
34 m_sensorPose(0, 0, 0), m_rx_buffer(40000), m_highSensMode(false),
35 m_reduced_fov(0), m_com_port(
""), m_ip_dir(
""), m_port_dir(10940),
36 m_I_am_owner_serial_port(false), m_timeStartUI(0),
37 m_timeStartSynchDelay(0), m_disable_firmware_timestamp(false),
38 m_intensity(false), m_scan_interval(0) {
57 const size_t N = strlen(str);
67 bool &outThereIsObservation,
69 outThereIsObservation =
false;
70 hardwareError =
false;
81 char rcv_status0, rcv_status1;
83 int expectedSize = nRanges * 3 + 4;
85 expectedSize += nRanges * 3;
97 if (rcv_status0 !=
'0' && rcv_status0 !=
'9') {
107 if ((
size_t)expectedSize !=
m_rcv_data.size()) {
109 << expectedSize <<
" data bytes, received " 111 hardwareError =
true;
117 if (do_timestamp_sync &&
119 do_timestamp_sync =
false;
123 if (do_timestamp_sync) {
157 for (
int i = 0; i < nRanges; i++) {
158 int b1 = (*ptr++) - 0x30;
159 int b2 = (*ptr++) - 0x30;
160 int b3 = (*ptr++) - 0x30;
162 int range_mm = ((
b1 << 12) | (
b2 << 6) |
b3);
167 range_mm >= 20 && (outObservation.
scan[i] <= outObservation.
maxRange));
170 int b4 = (*ptr++) - 0x30;
171 int b5 = (*ptr++) - 0x30;
172 int b6 = (*ptr++) - 0x30;
183 outThereIsObservation =
true;
196 configSource.
read_int(iniSection,
"HOKUYO_motorSpeed_rpm", 0);
198 configSource.
read_float(iniSection,
"pose_x", 0),
199 configSource.
read_float(iniSection,
"pose_y", 0),
200 configSource.
read_float(iniSection,
"pose_z", 0),
219 "Either COM_port or IP_DIR must be defined in the configuration file!");
221 "Both COM_port and IP_DIR set! Please, define only one of them.");
224 "A TCP/IP port number `PORT_DIR` must be specified for Ethernet " 253 if (COM !=
nullptr) {
260 std::this_thread::sleep_for(std::chrono::milliseconds(10));
263 std::this_thread::sleep_for(std::chrono::milliseconds(10));
267 std::this_thread::sleep_for(std::chrono::milliseconds(10));
269 std::this_thread::sleep_for(std::chrono::milliseconds(10));
273 if (COM !=
nullptr) {
282 if (COM !=
nullptr) {
285 std::this_thread::sleep_for(std::chrono::milliseconds(10));
288 std::this_thread::sleep_for(std::chrono::milliseconds(10));
291 std::this_thread::sleep_for(std::chrono::milliseconds(10));
322 const int half_range =
358 char rcv_status0, rcv_status1;
363 "[CHokuyoURG::setHighBaudrate] Changing baudrate to 115200...");
370 MRPT_LOG_ERROR(
"[CHokuyoURG::setHighBaudrate] Error waiting for response");
396 nRead = client->
readAsync(buf, to_read, 100, 10);
402 }
catch (std::exception &) {
434 }
while (i < verifLen);
445 if (rcv_status1 != 0x0A) {
460 if (nextChar != 0x0A)
471 bool lastWasLF =
false;
502 if (rcv_status0 !=
'0' &&
503 (rcv_status0 !=
'9' && rcv_status1 !=
'9')) {
505 << (
int)rcv_status0 <<
" after command: `" 516 }
catch (std::exception &e) {
525 char rcv_status0, rcv_status1;
529 MRPT_LOG_DEBUG(
"[CHokuyoURG::enableSCIP20] Changing protocol to SCIP2.0...");
545 char rcv_status0, rcv_status1;
550 MRPT_LOG_DEBUG(
"[CHokuyoURG::switchLaserOn] Switching laser ON...");
566 char rcv_status0, rcv_status1;
571 MRPT_LOG_DEBUG(
"[CHokuyoURG::switchLaserOff] Switching laser OFF...");
592 char rcv_status0, rcv_status1;
600 int motorSpeedCode = (600 - motoSpeed_rpm) / 6;
601 if (motorSpeedCode < 0 || motorSpeedCode > 10) {
603 <<
" Motorspeed must be in the range 540-600 rpm");
608 os::sprintf(cmd, 20,
"CR%02i\x0A", motorSpeedCode);
625 char rcv_status0, rcv_status1;
630 "[CHokuyoURG::setHighSensitivityMode] Setting HS mode to: %s...",
631 enabled ?
"true" :
"false");
657 char rcv_status0, rcv_status1;
661 MRPT_LOG_DEBUG(
"[CHokuyoURG::displayVersionInfo] Asking info...");
682 "\n------------- HOKUYO Scanner: Version Information ------\n" 685 "-------------------------------------------------------\n\n");
694 char rcv_status0, rcv_status1;
698 MRPT_LOG_DEBUG(
"[CHokuyoURG::displaySensorInfo] Asking for info...");
718 "\n------------- HOKUYO Scanner: Product Information ------\n" 721 "-------------------------------------------------------\n\n");
728 if (
nullptr != (ptr = strstr(&
m_rcv_data[0],
"DMAX:")))
729 out_data->
d_max = 0.001 * atoi(ptr + 5);
733 if (
nullptr != (ptr = strstr(&
m_rcv_data[0],
"DMIN:")))
734 out_data->
d_min = 0.001 * atoi(ptr + 5);
738 if (
nullptr != (ptr = strstr(&
m_rcv_data[0],
"ARES:")))
743 if (
nullptr != (ptr = strstr(&
m_rcv_data[0],
"SCAN:")))
748 if (
nullptr != (ptr = strstr(&
m_rcv_data[0],
"AMIN:")))
753 if (
nullptr != (ptr = strstr(&
m_rcv_data[0],
"AMAX:")))
758 if (
nullptr != (ptr = strstr(&
m_rcv_data[0],
"AFRT:")))
763 if (
nullptr != (ptr = strstr(&
m_rcv_data[0],
"MODL:"))) {
767 out_data->
model = aux;
776 char rcv_status0, rcv_status1;
780 MRPT_LOG_DEBUG(
"[CHokuyoURG::startScanningMode] Starting scanning mode...");
791 if (scan_interval > 9)
817 if (COM !=
nullptr) {
840 if (COM !=
nullptr) {
847 <<
": Serial port connection lost! Trying to reconnect...");
865 "No stream bound to the laser nor COM serial port or ip and " 866 "port provided in 'm_com_port','m_ip_dir' and 'm_port_dir'");
880 <<
" Cannot connect with the server '" 915 this->setMinLoggingLevel(mrpt::utils::LVL_DEBUG);
932 if (COM !=
nullptr) {
942 void *buf = malloc(
sizeof(
uint8_t) * to_read);
946 if (nRead != to_read)
948 "Error in purge buffers: read and expected number of bytes " uint64_t TTimeStamp
A system independent time type, it holds the the number of 100-nanosecond intervals since January 1...
void BASE_IMPEXP memcpy(void *dest, size_t destSize, const void *src, size_t copyCount) MRPT_NO_THROWS
An OS and compiler independent version of "memcpy".
size_t ReadBuffer(void *Buffer, size_t Count)
Reads a block of bytes from the stream into Buffer.
void setScanInterval(unsigned int skipScanCount)
Set the skip scan count (0 means send all scans).
std::string m_com_port
If set to non-empty, the serial port will be attempted to be opened automatically when this class is ...
uint32_t m_timeStartUI
Time of the first data packet, for synchronization purposes.
virtual ~CHokuyoURG()
Destructor: turns the laser off.
bool read_bool(const std::string §ion, const std::string &name, bool defaultValue, bool failIfNotFound=false) const
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
int m_firstRange
The first and last ranges to consider from the scan.
bool isOpen() const
Returns if port has been correctly open.
std::string m_rcv_data
temp buffer for incoming data packets
A communications serial port built as an implementation of a utils::CStream.
utils::CStream * m_stream
The I/O channel (will be NULL if not bound).
This namespace provides a OS-independent interface to many useful functions: filenames manipulation...
void setTimeouts(int ReadIntervalTimeout, int ReadTotalTimeoutMultiplier, int ReadTotalTimeoutConstant, int WriteTotalTimeoutMultiplier, int WriteTotalTimeoutConstant)
Changes the timeouts of the port, in milliseconds.
float read_float(const std::string §ion, const std::string &name, float defaultValue, bool failIfNotFound=false) const
bool m_intensity
Get intensity from lidar scan (default: false)
void setScanRange(const size_t i, const float val)
void loadConfig_sensorSpecific(const mrpt::utils::CConfigFileBase &configSource, const std::string &iniSection)
See the class documentation at the top for expected parameters.
std::string m_sensorLabel
See CGenericSensor.
size_t available() const
The maximum number of elements that can be written ("push") without rising an overflow error...
unsigned int getScanInterval() const
#define THROW_EXCEPTION(msg)
#define MRPT_LOG_DEBUG_FMT(_FMT_STRING,...)
size_t size() const
Return the number of elements available for read ("pop") in the buffer (this is NOT the maximum size ...
void WriteBuffer(const void *Buffer, size_t Count)
Writes a block of bytes to the stream from Buffer.
bool m_I_am_owner_serial_port
bool assureBufferHasBytes(const size_t nDesiredBytes)
Assures a minimum number of bytes in the input buffer, reading from the serial port only if required...
mrpt::system::TTimeStamp now()
A shortcut for system::getCurrentTime.
Contains classes for various device interfaces.
void setScanHasIntensity(bool setHasIntensityFlag)
Marks this scan as having or not intensity data.
std::string read_string(const std::string §ion, const std::string &name, const std::string &defaultValue, bool failIfNotFound=false) const
void filterByExclusionAreas(mrpt::obs::CObservation2DRangeScan &obs) const
Mark as invalid those points which (x,y) coordinates fall within the exclusion polygons.
bool ensureStreamIsOpen()
Returns true if there is a valid stream bound to the laser scanner, otherwise it first try to open th...
float stdError
The "sigma" error of the device in meters, used while inserting the scan in an occupancy grid...
#define MRPT_LOG_ERROR_FMT(_FMT_STRING,...)
void filterByExclusionAngles(mrpt::obs::CObservation2DRangeScan &obs) const
Mark as invalid those ranges in a set of forbiden angle ranges.
bool switchLaserOn()
Switchs the laser on.
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.
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...
This class allows loading and storing values and vectors of different types from a configuration text...
std::string m_lastSentMeasCmd
The last sent measurement command (MDXXX), including the last 0x0A.
mrpt::gui::CDisplayWindow3D::Ptr m_win
bool m_disable_firmware_timestamp
float maxRange
The maximum range allowed by the device, in meters (e.g. 80m, 50m,...)
const int MINIMUM_PACKETS_TO_SET_TIMESTAMP_REFERENCE
int read_int(const std::string §ion, const std::string &name, int defaultValue, bool failIfNotFound=false) const
void setConfig(int baudRate, int parity=0, int bits=8, int nStopBits=1, bool enableFlowControl=false)
Changes the configuration of the port.
Used in CHokuyoURG::displayVersionInfo.
bool isConnected()
Returns true if this objects represents a successfully connected socket.
void setScanIntensity(const size_t i, const int val)
bool setIntensityMode(bool enabled)
If true scans will capture intensity.
bool displayVersionInfo()
Ask to the device, and print to the debug stream, details about the firmware version,serial number,...
bool displaySensorInfo(CHokuyoURG::TSensorInfo *out_data=NULL)
Ask to the device, and print to the debug stream, details about the sensor model. ...
This namespace contains representation of robot actions and observations.
bool receiveResponse(char &rcv_status0, char &rcv_status1)
Waits for a response from the device.
#define MRPT_LOG_DEBUG(_STRING)
int m_timeStartSynchDelay
Counter to discard to first few packets before setting the correspondence between device and computer...
bool enableSCIP20()
Enables the SCIP2.0 protocol (this must be called at the very begining!).
void bindIO(mrpt::utils::CStream *streamIO)
Binds the object to a given I/O channel.
mrpt::system::TTimeStamp m_timeStartTT
void loadCommonParams(const mrpt::utils::CConfigFileBase &configSource, const std::string &iniSection)
Should be call by derived classes at "loadConfig" (loads exclusion areas AND exclusion angles)...
GLsizei const GLchar ** string
#define IMPLEMENTS_GENERIC_SENSOR(class_name, NameSpace)
This must be inserted in all CGenericSensor classes implementation files:
T pop()
Retrieve an element from the buffer.
bool switchLaserOff()
Switchs the laser off.
int m_motorSpeed_rpm
The motor speed (default=600rpm)
std::string sensorLabel
An arbitrary label that can be used to identify the sensor.
void purgeBuffers()
Empties the RX buffers of the serial port.
mrpt::system::TTimeStamp timestamp
The associated UTC time-stamp. Where available, this should contain the accurate satellite-based time...
void connect(const std::string &remotePartAddress, unsigned short remotePartTCPPort, unsigned int timeout_ms=0)
Establishes a connection with a remote part.
int scan_first
First, last, and front step of the scanner angular span.
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
#define MRPT_LOG_INFO_STREAM(__CONTENTS)
bool startScanningMode()
Start the continuous scanning mode, using parameters stored in the object (loaded from the ...
#define MRPT_LOAD_HERE_CONFIG_VAR(variableName, variableType, targetVariable, configFileObject, sectionNameStr)
void purgeBuffers()
Purge tx and rx buffers.
size_t getReadPendingBytes()
Return the number of bytes already in the receive queue (they can be read without waiting) ...
void open()
Open the port.
void resizeScan(const size_t len)
Resizes all data vectors to allocate a given number of scan rays.
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...
bool turnOn()
Enables the scanning mode (which may depend on the specific laser device); this must be called before...
void processPreview(const mrpt::obs::CObservation2DRangeScan &obs)
Must be called inside the capture method to allow optional GUI preview of scans.
bool setMotorSpeed(int motoSpeed_rpm)
Changes the motor speed in rpm's (default 600rpm)
double d_min
Min/Max ranges, in meters.
The namespace for 3D scene representation and rendering.
This software driver implements the protocol SCIP-2.0 for interfacing HOKUYO URG/UTM/UXM/UST laser sc...
float aperture
The "aperture" or field-of-view of the range finder, in radians (typically M_PI = 180 degrees)...
size_t capacity() const
Return the maximum capacity of the buffer.
void initialize()
Turns the laser on.
void doProcessSimple(bool &outThereIsObservation, mrpt::obs::CObservation2DRangeScan &outObservation, bool &hardwareError)
Specific laser scanner "software drivers" must process here new data from the I/O stream...
bool setHighBaudrate()
Passes to 115200bps bitrate.
poses::CPose3D m_sensorPose
The sensor 6D pose:
unsigned int m_scan_interval
int BASE_IMPEXP sprintf(char *buf, size_t bufSize, const char *format,...) MRPT_NO_THROWS MRPT_printf_format_check(3
An OS-independent version of sprintf (Notice the bufSize param, which may be ignored in some compiler...
#define __CURRENT_FUNCTION_NAME__
A macro for obtaining the name of the current function:
TSensorInfo m_sensor_info
The information gathered when the laser is first open.
mrpt::system::TTimeStamp BASE_IMPEXP secondsToTimestamp(const double nSeconds)
Transform a time interval (in seconds) into TTimeStamp (e.g.
A TCP socket that can be connected to a TCP server, implementing MRPT's CStream interface for passing...
bool m_highSensMode
High sensitivity [HS] mode (default: false)
#define MRPT_LOG_ERROR(_STRING)
std::string model
The sensor model.
mrpt::utils::ContainerReadOnlyProxyAccessor< std::vector< float > > scan
The range values of the scan, in meters. Must have same length than validRange.
double m_reduced_fov
Used to reduce artificially the interval of scan ranges.
mrpt::utils::circular_buffer< uint8_t > m_rx_buffer
Auxiliary buffer for readings.
int motor_speed_rpm
Standard motor speed, rpm.
void push_many(T *array_elements, size_t count)
Insert an array of elements in the buffer.
unsigned __int32 uint32_t
#define MRPT_LOG_DEBUG_STREAM(__CONTENTS)
bool turnOff()
Disables the scanning mode (this can be used to turn the device in low energy mode, if available)
#define ASSERTMSG_(f, __ERROR_MSG)
#define MRPT_LOG_ERROR_STREAM(__CONTENTS)
bool setHighSensitivityMode(bool enabled)
Changes the high sensitivity mode (HS) (default: false)
mrpt::poses::CPose3D sensorPose
The 6D pose of the sensor on the robot at the moment of starting the scan.
void sendCmd(const char *str)
bool rightToLeft
The scanning direction: true=counterclockwise; false=clockwise.
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...
void setScanRangeValidity(const size_t i, const bool val)
int scans_per_360deg
Number of measuremens per 360 degrees.