MRPT  2.0.3
CHokuyoURG.h
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2020, 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 #pragma once
10 
14 #include <mrpt/poses/CPose3D.h>
15 
16 namespace mrpt::hwdrivers
17 {
18 /** This software driver implements the protocol SCIP-2.0 for interfacing HOKUYO
19  * URG/UTM/UXM/UST laser scanners (USB or Ethernet).
20  * Refer to the example code
21  * [HOKUYO_laser_test](http://www.mrpt.org/tutorials/mrpt-examples/example_hokuyo_urgutm_laser_scanner/)
22  * and to example rawlog-grabber [config
23  * files](https://github.com/MRPT/mrpt/tree/master/share/mrpt/config_files/rawlog-grabber)
24  *
25  * See also the application "rawlog-grabber" for a ready-to-use application to
26  * gather data from the scanner.
27  *
28  * \code
29  * PARAMETERS IN THE ".INI"-LIKE CONFIGURATION STRINGS:
30  * -------------------------------------------------------
31  * [supplied_section_name]
32  * HOKUYO_motorSpeed_rpm=600
33  * #HOKUYO_HS_mode = false // Optional (un-comment line if used):
34  * Set/unset the High-sensitivity mode (not on all models/firmwares!)
35  *
36  * # Uncomment serial port or IP address, depending on the Hokuyo model
37  * (serial/USB vs. Ethernet):
38  * COM_port_WIN = COM3 // Serial port name in Windows
39  * COM_port_LIN = ttyS0 // Serial port name in GNU/Linux
40  * #IP_DIR = 192.168.0.10 // Uncommented this and "PORT_DIR" if the
41  * used HOKUYO is connected by Ethernet instead of USB
42  * #PORT_DIR = 10940 // Default value: 10940
43  *
44  * pose_x=0.21 // Laser range scaner 3D position in the robot (meters)
45  * pose_y=0
46  * pose_z=0.34
47  * pose_yaw=0 // Angles in degrees
48  * pose_pitch=0
49  * pose_roll=0
50  *
51  * #disable_firmware_timestamp = true // Uncomment to use PC time instead
52  * of laser time
53  *
54  * # Optional: reduced FOV:
55  * # reduced_fov = 25 // Deg
56  *
57  * # Sets decimation of scans directly at the Hokuyo scanner.
58  * # 0=means send all scans, 1=means send 50% of scans, etc.
59  * # scan_interval = 0
60  *
61  * #preview = true // Enable GUI visualization of captured data
62  *
63  * # Optional: Exclusion zones to avoid the robot seeing itself:
64  * #exclusionZone1_x = 0.20 0.30 0.30 0.20
65  * #exclusionZone1_y = 0.20 0.30 0.30 0.20
66  *
67  * # Optional: Exclusion zones to avoid the robot seeing itself:
68  * #exclusionAngles1_ini = 20 // Deg
69  * #exclusionAngles1_end = 25 // Deg
70  *
71  * \endcode
72  * \ingroup mrpt_hwdrivers_grp
73  */
75 {
77  public:
78  /** Used in CHokuyoURG::displayVersionInfo */
79  struct TSensorInfo
80  {
81  /** The sensor model */
82  std::string model;
83  /** Min/Max ranges, in meters. */
84  double d_min{0}, d_max{0};
85  /** Number of measuremens per 360 degrees. */
87  /** First, last, and front step of the scanner angular span. */
89  /** Standard motor speed, rpm. */
91  };
92 
93  private:
94  /** The first and last ranges to consider from the scan. */
95  int m_firstRange{44}, m_lastRange{725};
96  /** The motor speed (default=600rpm) */
98  /** The sensor 6D pose: */
99  poses::CPose3D m_sensorPose{0, 0, 0, 0, 0, 0};
100  /** Auxiliary buffer for readings */
102 
103  /** The last sent measurement command (MDXXX), including the last 0x0A. */
104  std::string m_lastSentMeasCmd;
105 
106  /** High sensitivity [HS] mode (default: false) */
107  bool m_highSensMode{false};
109 
110  /** Enables the SCIP2.0 protocol (this must be called at the very
111  * begining!).
112  * \return false on any error
113  */
114  bool enableSCIP20();
115 
116  /** Passes to 115200bps bitrate.
117  * \return false on any error
118  */
119  bool setHighBaudrate();
120 
121  /** Switchs the laser on.
122  * \return false on any error
123  */
124  bool switchLaserOn();
125 
126  /** Switchs the laser off
127  * \return false on any error
128  */
129  bool switchLaserOff();
130 
131  /** Changes the motor speed in rpm's (default 600rpm)
132  * \return false on any error
133  */
134  bool setMotorSpeed(int motoSpeed_rpm);
135 
136  /** Ask to the device, and print to the debug stream, details about the
137  * firmware version,serial number,...
138  * \return false on any error
139  */
140  bool displayVersionInfo();
141 
142  /** Ask to the device, and print to the debug stream, details about the
143  * sensor model.
144  * It also optionally saves all the information in an user supplied data
145  * structure "out_data".
146  * \return false on any error
147  */
148  bool displaySensorInfo(CHokuyoURG::TSensorInfo* out_data = nullptr);
149 
150  /** Start the continuous scanning mode, using parameters stored in the
151  * object (loaded
152  * from the .ini file). Maps to SCIP2.0 commands MD (no intensity) or ME
153  * (intensity).
154  * After this command the device will start to send scans until
155  * switchLaserOff() is called.
156  * \return false on any error
157  */
158  bool startScanningMode();
159 
160  /** Turns the laser on */
161  void initialize() override;
162 
163  /** Parses the response from the device from raw bytes in m_rx_buffer, and
164  * stored the received frame in m_rcv_data. Status codes are stored in
165  * m_rcv_status0 and m_rcv_status1.
166  * \return false on any error or if received frame is incomplete and needs
167  * more input bytes.
168  */
169  bool parseResponse();
170 
171  /** Assures a minimum number of bytes in the input buffer, reading from the
172  * serial port only if required.
173  * \return false if the number of bytes are not available, even after
174  * trying to fetch more data from the serial port.
175  */
176  bool ensureBufferHasBytes(const size_t nDesiredBytes);
177 
178  public:
179  /** Constructor
180  */
181  CHokuyoURG();
182 
183  /** Destructor: turns the laser off */
184  ~CHokuyoURG() override;
185 
186  /** Specific laser scanner "software drivers" must process here new data
187  * from the I/O stream, and, if a whole scan has arrived, return it.
188  * This method will be typically called in a different thread than other
189  * methods, and will be called in a timely fashion.
190  */
191  void doProcessSimple(
192  bool& outThereIsObservation,
193  mrpt::obs::CObservation2DRangeScan& outObservation,
194  bool& hardwareError) override;
195 
196  /** Enables the scanning mode (which may depend on the specific laser
197  * device); this must be called before asking for observations to assure
198  * that the protocol has been initializated.
199  * \return If everything works "true", or "false" if there is any error.
200  */
201  bool turnOn() override;
202 
203  /** Disables the scanning mode (this can be used to turn the device in low
204  * energy mode, if available)
205  * \return If everything works "true", or "false" if there is any error.
206  */
207  bool turnOff() override;
208 
209  /** Empties the RX buffers of the serial port */
210  void purgeBuffers();
211 
212  /** If set to non-empty, the serial port will be attempted to be opened
213  * automatically when this class is first used to request data from the
214  * laser. */
215  void setSerialPort(const std::string& port_name) { m_com_port = port_name; }
216  /** Set the ip direction and port to connect using Ethernet communication */
217  void setIPandPort(const std::string& ip, const unsigned int& port)
218  {
219  m_ip_dir = ip;
220  m_port_dir = port;
221  }
222 
223  /** Returns the currently set serial port \sa setSerialPort */
224  const std::string getSerialPort() { return m_com_port; }
225  /** If called (before calling "turnOn"), the field of view of the laser is
226  * reduced to the given range (in radians), discarding the rest of measures.
227  * Call with "0" to disable this reduction again (the default).
228  */
229  void setReducedFOV(const double fov) { m_reduced_fov = fov; }
230  /** Changes the high sensitivity mode (HS) (default: false)
231  * \return false on any error
232  */
233  bool setHighSensitivityMode(bool enabled);
234 
235  /** If true scans will capture intensity. (default: false)
236  * Should not be called while scanning.
237  * \return false on any error
238  */
239  bool setIntensityMode(bool enabled);
240 
241  /** Set the skip scan count (0 means send all scans).
242  * Must be set before initialize()
243  */
244  void setScanInterval(unsigned int skipScanCount);
245  unsigned int getScanInterval() const;
246 
247  void sendCmd(const char* str);
248 
249  protected:
250  /** temp buffer for incoming data packets */
251  std::string m_rcv_data;
252  char m_rcv_status0 = '\0', m_rcv_status1 = '\0';
253 
254  /** Returns true if there is a valid stream bound to the laser scanner,
255  * otherwise it first try to open the serial port "m_com_port"
256  */
257  bool ensureStreamIsOpen();
258 
259  /** Called upon dtor, or when trying to recover from a disconnected sensor
260  */
261  void closeStreamConnection();
262 
263  /** Used to reduce artificially the interval of scan ranges. */
264  double m_reduced_fov{0};
265 
266  /** If set to non-empty, the serial port will be attempted to be opened
267  * automatically when this class is first used to request data from the
268  * laser. */
269  std::string m_com_port{};
270 
271  /** If set to non-empty and m_port_dir too, the program will try to connect
272  * to a Hokuyo using Ethernet communication */
273  std::string m_ip_dir{};
274  /** If set to non-empty and m_ip_dir too, the program will try to connect to
275  * a Hokuyo using Ethernet communication */
276  unsigned int m_port_dir{10940};
277 
278  /** The information gathered when the laser is first open */
280 
281  /** Time of the first data packet, for synchronization purposes. */
282  uint32_t m_timeStartUI{0};
283  /** Counter to discard to first few packets before setting the
284  * correspondence between device and computer timestamps. */
288  /** Get intensity from lidar scan (default: false) */
289  bool m_intensity{false};
290  unsigned int m_scan_interval{0};
291 
292  /** See the class documentation at the top for expected parameters */
294  const mrpt::config::CConfigFileBase& configSource,
295  const std::string& iniSection) override;
296 
297 }; // End of class
298 
299 } // namespace mrpt::hwdrivers
mrpt::hwdrivers::CHokuyoURG::m_rx_buffer
mrpt::containers::circular_buffer< uint8_t > m_rx_buffer
Auxiliary buffer for readings.
Definition: CHokuyoURG.h:101
mrpt::obs::CObservation2DRangeScan
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
Definition: CObservation2DRangeScan.h:54
mrpt::hwdrivers::CHokuyoURG::TSensorInfo::scan_last
int scan_last
Definition: CHokuyoURG.h:88
mrpt::hwdrivers::CHokuyoURG::m_lastRange
int m_lastRange
Definition: CHokuyoURG.h:95
mrpt::containers::circular_buffer< uint8_t >
mrpt::hwdrivers::CHokuyoURG::m_timeStartUI
uint32_t m_timeStartUI
Time of the first data packet, for synchronization purposes.
Definition: CHokuyoURG.h:282
mrpt::hwdrivers::CHokuyoURG::TSensorInfo::d_max
double d_max
Definition: CHokuyoURG.h:84
mrpt::hwdrivers::CHokuyoURG::TSensorInfo
Used in CHokuyoURG::displayVersionInfo.
Definition: CHokuyoURG.h:79
mrpt::hwdrivers::CHokuyoURG::closeStreamConnection
void closeStreamConnection()
Called upon dtor, or when trying to recover from a disconnected sensor.
Definition: CHokuyoURG.cpp:45
mrpt::hwdrivers::CHokuyoURG::TSensorInfo::scan_front
int scan_front
Definition: CHokuyoURG.h:88
mrpt::hwdrivers::CHokuyoURG::m_timeStartSynchDelay
int m_timeStartSynchDelay
Counter to discard to first few packets before setting the correspondence between device and computer...
Definition: CHokuyoURG.h:285
mrpt::hwdrivers::CHokuyoURG::m_firstRange
int m_firstRange
The first and last ranges to consider from the scan.
Definition: CHokuyoURG.h:95
mrpt::hwdrivers
Contains classes for various device interfaces.
Definition: C2DRangeFinderAbstract.h:19
mrpt::hwdrivers::CHokuyoURG::m_rcv_status1
char m_rcv_status1
Definition: CHokuyoURG.h:252
DEFINE_GENERIC_SENSOR
#define DEFINE_GENERIC_SENSOR(class_name)
This declaration must be inserted in all CGenericSensor classes definition, within the class declarat...
Definition: CGenericSensor.h:302
mrpt::hwdrivers::CHokuyoURG::~CHokuyoURG
~CHokuyoURG() override
Destructor: turns the laser off.
Definition: CHokuyoURG.cpp:32
mrpt::hwdrivers::CHokuyoURG::enableSCIP20
bool enableSCIP20()
Enables the SCIP2.0 protocol (this must be called at the very begining!).
Definition: CHokuyoURG.cpp:600
mrpt::hwdrivers::CHokuyoURG::turnOff
bool turnOff() override
Disables the scanning mode (this can be used to turn the device in low energy mode,...
Definition: CHokuyoURG.cpp:388
mrpt::hwdrivers::CHokuyoURG::TSensorInfo::d_min
double d_min
Min/Max ranges, in meters.
Definition: CHokuyoURG.h:84
mrpt::hwdrivers::CHokuyoURG::ensureStreamIsOpen
bool ensureStreamIsOpen()
Returns true if there is a valid stream bound to the laser scanner, otherwise it first try to open th...
Definition: CHokuyoURG.cpp:897
CDisplayWindow3D.h
mrpt::hwdrivers::CHokuyoURG::m_timeStartTT
mrpt::system::TTimeStamp m_timeStartTT
Definition: CHokuyoURG.h:286
mrpt::hwdrivers::CHokuyoURG::m_motorSpeed_rpm
int m_motorSpeed_rpm
The motor speed (default=600rpm)
Definition: CHokuyoURG.h:97
mrpt::hwdrivers::CHokuyoURG::turnOn
bool turnOn() override
Enables the scanning mode (which may depend on the specific laser device); this must be called before...
Definition: CHokuyoURG.cpp:277
mrpt::hwdrivers::CHokuyoURG::getScanInterval
unsigned int getScanInterval() const
Definition: CHokuyoURG.cpp:668
mrpt::hwdrivers::CHokuyoURG::m_port_dir
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...
Definition: CHokuyoURG.h:276
mrpt::hwdrivers::CHokuyoURG::m_disable_firmware_timestamp
bool m_disable_firmware_timestamp
Definition: CHokuyoURG.h:287
mrpt::hwdrivers::C2DRangeFinderAbstract
This is the base, abstract class for "software drivers" interfaces to 2D scanners (laser range finder...
Definition: C2DRangeFinderAbstract.h:40
mrpt::hwdrivers::CHokuyoURG::switchLaserOn
bool switchLaserOn()
Switchs the laser on.
Definition: CHokuyoURG.cpp:622
mrpt::hwdrivers::CHokuyoURG::purgeBuffers
void purgeBuffers()
Empties the RX buffers of the serial port.
Definition: CHokuyoURG.cpp:1037
mrpt::hwdrivers::CHokuyoURG::setSerialPort
void setSerialPort(const std::string &port_name)
If set to non-empty, the serial port will be attempted to be opened automatically when this class is ...
Definition: CHokuyoURG.h:215
mrpt::hwdrivers::CHokuyoURG::loadConfig_sensorSpecific
void loadConfig_sensorSpecific(const mrpt::config::CConfigFileBase &configSource, const std::string &iniSection) override
See the class documentation at the top for expected parameters.
Definition: CHokuyoURG.cpp:218
mrpt::config::CConfigFileBase
This class allows loading and storing values and vectors of different types from a configuration text...
Definition: config/CConfigFileBase.h:44
mrpt::hwdrivers::CHokuyoURG::ensureBufferHasBytes
bool ensureBufferHasBytes(const size_t nDesiredBytes)
Assures a minimum number of bytes in the input buffer, reading from the serial port only if required.
Definition: CHokuyoURG.cpp:421
circular_buffer.h
mrpt::hwdrivers::CHokuyoURG::m_scan_interval
unsigned int m_scan_interval
Definition: CHokuyoURG.h:290
mrpt::poses::CPose3D
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
mrpt::hwdrivers::CHokuyoURG::doProcessSimple
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,...
Definition: CHokuyoURG.cpp:69
mrpt::hwdrivers::CHokuyoURG::m_lastSentMeasCmd
std::string m_lastSentMeasCmd
The last sent measurement command (MDXXX), including the last 0x0A.
Definition: CHokuyoURG.h:104
mrpt::hwdrivers::CHokuyoURG::m_win
mrpt::gui::CDisplayWindow3D::Ptr m_win
Definition: CHokuyoURG.h:108
mrpt::hwdrivers::CHokuyoURG::initialize
void initialize() override
Turns the laser on.
Definition: CHokuyoURG.cpp:1024
mrpt::gui::CDisplayWindow3D::Ptr
std::shared_ptr< CDisplayWindow3D > Ptr
Definition: CDisplayWindow3D.h:120
mrpt::hwdrivers::CHokuyoURG::setIntensityMode
bool setIntensityMode(bool enabled)
If true scans will capture intensity.
Definition: CHokuyoURG.cpp:733
mrpt::system::TTimeStamp
mrpt::Clock::time_point TTimeStamp
A system independent time type, it holds the the number of 100-nanosecond intervals since January 1,...
Definition: datetime.h:40
mrpt::hwdrivers::CHokuyoURG::m_reduced_fov
double m_reduced_fov
Used to reduce artificially the interval of scan ranges.
Definition: CHokuyoURG.h:264
mrpt::hwdrivers::CHokuyoURG::CHokuyoURG
CHokuyoURG()
Constructor.
Definition: CHokuyoURG.cpp:30
mrpt::hwdrivers::CHokuyoURG::displayVersionInfo
bool displayVersionInfo()
Ask to the device, and print to the debug stream, details about the firmware version,...
Definition: CHokuyoURG.cpp:739
mrpt::hwdrivers::CHokuyoURG::displaySensorInfo
bool displaySensorInfo(CHokuyoURG::TSensorInfo *out_data=nullptr)
Ask to the device, and print to the debug stream, details about the sensor model.
Definition: CHokuyoURG.cpp:777
mrpt::hwdrivers::CHokuyoURG::m_rcv_status0
char m_rcv_status0
Definition: CHokuyoURG.h:252
mrpt::hwdrivers::CHokuyoURG::TSensorInfo::scan_first
int scan_first
First, last, and front step of the scanner angular span.
Definition: CHokuyoURG.h:88
mrpt::hwdrivers::CHokuyoURG::sendCmd
void sendCmd(const char *str)
Definition: CHokuyoURG.cpp:54
CPose3D.h
mrpt::hwdrivers::CHokuyoURG::setHighSensitivityMode
bool setHighSensitivityMode(bool enabled)
Changes the high sensitivity mode (HS) (default: false)
Definition: CHokuyoURG.cpp:705
mrpt::hwdrivers::CHokuyoURG::m_ip_dir
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...
Definition: CHokuyoURG.h:273
mrpt::hwdrivers::CHokuyoURG::parseResponse
bool parseResponse()
Parses the response from the device from raw bytes in m_rx_buffer, and stored the received frame in m...
Definition: CHokuyoURG.cpp:446
mrpt::hwdrivers::CHokuyoURG::getSerialPort
const std::string getSerialPort()
Returns the currently set serial port.
Definition: CHokuyoURG.h:224
mrpt::hwdrivers::CHokuyoURG::setReducedFOV
void setReducedFOV(const double fov)
If called (before calling "turnOn"), the field of view of the laser is reduced to the given range (in...
Definition: CHokuyoURG.h:229
mrpt::hwdrivers::CHokuyoURG::TSensorInfo::scans_per_360deg
int scans_per_360deg
Number of measuremens per 360 degrees.
Definition: CHokuyoURG.h:86
mrpt::hwdrivers::CHokuyoURG
This software driver implements the protocol SCIP-2.0 for interfacing HOKUYO URG/UTM/UXM/UST laser sc...
Definition: CHokuyoURG.h:74
mrpt::hwdrivers::CHokuyoURG::m_com_port
std::string m_com_port
If set to non-empty, the serial port will be attempted to be opened automatically when this class is ...
Definition: CHokuyoURG.h:269
mrpt::hwdrivers::CHokuyoURG::TSensorInfo::motor_speed_rpm
int motor_speed_rpm
Standard motor speed, rpm.
Definition: CHokuyoURG.h:90
mrpt::hwdrivers::CHokuyoURG::switchLaserOff
bool switchLaserOff()
Switchs the laser off.
Definition: CHokuyoURG.cpp:643
mrpt::hwdrivers::CHokuyoURG::setIPandPort
void setIPandPort(const std::string &ip, const unsigned int &port)
Set the ip direction and port to connect using Ethernet communication.
Definition: CHokuyoURG.h:217
mrpt::hwdrivers::CHokuyoURG::TSensorInfo::model
std::string model
The sensor model.
Definition: CHokuyoURG.h:82
mrpt::hwdrivers::CHokuyoURG::m_rcv_data
std::string m_rcv_data
temp buffer for incoming data packets
Definition: CHokuyoURG.h:251
mrpt::hwdrivers::CHokuyoURG::setMotorSpeed
bool setMotorSpeed(int motoSpeed_rpm)
Changes the motor speed in rpm's (default 600rpm)
Definition: CHokuyoURG.cpp:669
mrpt::hwdrivers::CHokuyoURG::startScanningMode
bool startScanningMode()
Start the continuous scanning mode, using parameters stored in the object (loaded from the ....
Definition: CHokuyoURG.cpp:863
mrpt::hwdrivers::CHokuyoURG::setScanInterval
void setScanInterval(unsigned int skipScanCount)
Set the skip scan count (0 means send all scans).
Definition: CHokuyoURG.cpp:664
mrpt::hwdrivers::CHokuyoURG::m_sensor_info
TSensorInfo m_sensor_info
The information gathered when the laser is first open.
Definition: CHokuyoURG.h:279
mrpt::hwdrivers::CHokuyoURG::m_intensity
bool m_intensity
Get intensity from lidar scan (default: false)
Definition: CHokuyoURG.h:289
mrpt::hwdrivers::CHokuyoURG::setHighBaudrate
bool setHighBaudrate()
Passes to 115200bps bitrate.
Definition: CHokuyoURG.cpp:396
mrpt::hwdrivers::CHokuyoURG::m_highSensMode
bool m_highSensMode
High sensitivity [HS] mode (default: false)
Definition: CHokuyoURG.h:107
C2DRangeFinderAbstract.h
mrpt::hwdrivers::CHokuyoURG::m_sensorPose
poses::CPose3D m_sensorPose
The sensor 6D pose:
Definition: CHokuyoURG.h:99



Page generated by Doxygen 1.8.17 for MRPT 2.0.3 at Fri May 29 13:06:46 UTC 2020