MRPT  1.9.9
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
void setScanInterval(unsigned int skipScanCount)
Set the skip scan count (0 means send all scans).
Definition: CHokuyoURG.cpp:664
const std::string getSerialPort()
Returns the currently set serial port.
Definition: CHokuyoURG.h:224
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
uint32_t m_timeStartUI
Time of the first data packet, for synchronization purposes.
Definition: CHokuyoURG.h:282
int m_firstRange
The first and last ranges to consider from the scan.
Definition: CHokuyoURG.h:95
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
std::string m_rcv_data
temp buffer for incoming data packets
Definition: CHokuyoURG.h:251
bool m_intensity
Get intensity from lidar scan (default: false)
Definition: CHokuyoURG.h:289
unsigned int getScanInterval() const
Definition: CHokuyoURG.cpp:668
mrpt::containers::circular_buffer< uint8_t > m_rx_buffer
Auxiliary buffer for readings.
Definition: CHokuyoURG.h:101
~CHokuyoURG() override
Destructor: turns the laser off.
Definition: CHokuyoURG.cpp:32
Contains classes for various device interfaces.
bool turnOn() override
Enables the scanning mode (which may depend on the specific laser device); this must be called before...
Definition: CHokuyoURG.cpp:277
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
bool switchLaserOn()
Switchs the laser on.
Definition: CHokuyoURG.cpp:622
void closeStreamConnection()
Called upon dtor, or when trying to recover from a disconnected sensor.
Definition: CHokuyoURG.cpp:45
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
std::string m_lastSentMeasCmd
The last sent measurement command (MDXXX), including the last 0x0A.
Definition: CHokuyoURG.h:104
mrpt::gui::CDisplayWindow3D::Ptr m_win
Definition: CHokuyoURG.h:108
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
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
This class allows loading and storing values and vectors of different types from a configuration text...
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
bool turnOff() override
Disables the scanning mode (this can be used to turn the device in low energy mode, if available)
Definition: CHokuyoURG.cpp:388
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
Used in CHokuyoURG::displayVersionInfo.
Definition: CHokuyoURG.h:79
bool setIntensityMode(bool enabled)
If true scans will capture intensity.
Definition: CHokuyoURG.cpp:733
bool displayVersionInfo()
Ask to the device, and print to the debug stream, details about the firmware version,serial number,...
Definition: CHokuyoURG.cpp:739
int m_timeStartSynchDelay
Counter to discard to first few packets before setting the correspondence between device and computer...
Definition: CHokuyoURG.h:285
bool enableSCIP20()
Enables the SCIP2.0 protocol (this must be called at the very begining!).
Definition: CHokuyoURG.cpp:600
mrpt::system::TTimeStamp m_timeStartTT
Definition: CHokuyoURG.h:286
bool switchLaserOff()
Switchs the laser off.
Definition: CHokuyoURG.cpp:643
int m_motorSpeed_rpm
The motor speed (default=600rpm)
Definition: CHokuyoURG.h:97
#define DEFINE_GENERIC_SENSOR(class_name)
This declaration must be inserted in all CGenericSensor classes definition, within the class declarat...
This is the base, abstract class for "software drivers" interfaces to 2D scanners (laser range finder...
void purgeBuffers()
Empties the RX buffers of the serial port.
int scan_first
First, last, and front step of the scanner angular span.
Definition: CHokuyoURG.h:88
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
bool startScanningMode()
Start the continuous scanning mode, using parameters stored in the object (loaded from the ...
Definition: CHokuyoURG.cpp:863
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
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
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
void initialize() override
Turns the laser on.
bool setMotorSpeed(int motoSpeed_rpm)
Changes the motor speed in rpm&#39;s (default 600rpm)
Definition: CHokuyoURG.cpp:669
double d_min
Min/Max ranges, in meters.
Definition: CHokuyoURG.h:84
This software driver implements the protocol SCIP-2.0 for interfacing HOKUYO URG/UTM/UXM/UST laser sc...
Definition: CHokuyoURG.h:74
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
bool setHighBaudrate()
Passes to 115200bps bitrate.
Definition: CHokuyoURG.cpp:396
poses::CPose3D m_sensorPose
The sensor 6D pose:
Definition: CHokuyoURG.h:99
unsigned int m_scan_interval
Definition: CHokuyoURG.h:290
TSensorInfo m_sensor_info
The information gathered when the laser is first open.
Definition: CHokuyoURG.h:279
bool m_highSensMode
High sensitivity [HS] mode (default: false)
Definition: CHokuyoURG.h:107
std::string model
The sensor model.
Definition: CHokuyoURG.h:82
double m_reduced_fov
Used to reduce artificially the interval of scan ranges.
Definition: CHokuyoURG.h:264
int motor_speed_rpm
Standard motor speed, rpm.
Definition: CHokuyoURG.h:90
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
bool setHighSensitivityMode(bool enabled)
Changes the high sensitivity mode (HS) (default: false)
Definition: CHokuyoURG.cpp:705
void sendCmd(const char *str)
Definition: CHokuyoURG.cpp:54
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
int scans_per_360deg
Number of measuremens per 360 degrees.
Definition: CHokuyoURG.h:86



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: c7a3bec24 Sun Mar 29 18:33:13 2020 +0200 at dom mar 29 18:50:38 CEST 2020