SickLMS1xx.hh
Go to the documentation of this file.
1 
17 #ifndef SICK_LMS_1XX_HH
18 #define SICK_LMS_1XX_HH
19 
20 /* Macros */
21 #define DEFAULT_SICK_LMS_1XX_IP_ADDRESS "192.168.0.1"
22 #define DEFAULT_SICK_LMS_1XX_TCP_PORT (2111)
23 #define DEFAULT_SICK_LMS_1XX_CONNECT_TIMEOUT (1000000)
24 #define DEFAULT_SICK_LMS_1XX_MESSAGE_TIMEOUT (5000000)
25 #define DEFAULT_SICK_LMS_1XX_STATUS_TIMEOUT (60000000)
26 
27 #define SICK_LMS_1XX_SCAN_AREA_MIN_ANGLE (-450000)
28 #define SICK_LMS_1XX_SCAN_AREA_MAX_ANGLE (2250000)
29 
30 /* Definition dependencies */
31 #include <string>
32 #include <arpa/inet.h>
33 
34 #include "SickLIDAR.hh"
36 #include "SickLMS1xxMessage.hh"
37 #include "SickException.hh"
38 
43 namespace SickToolbox {
44 
50  class SickLMS1xx : public SickLIDAR< SickLMS1xxBufferMonitor, SickLMS1xxMessage > {
51 
52  public:
53 
54  static const int SICK_LMS_1XX_MAX_NUM_MEASUREMENTS = 1082;
55 
62 
71 
72  };
73 
80 
88 
89  };
90 
97 
101 
102  };
103 
110 
114 
115  };
116 
118  SickLMS1xx( const std::string sick_ip_address = DEFAULT_SICK_LMS_1XX_IP_ADDRESS,
119  const uint16_t sick_tcp_port = DEFAULT_SICK_LMS_1XX_TCP_PORT );
120 
122  void Initialize( const bool disp_banner = true ) throw( SickIOException, SickThreadException, SickTimeoutException, SickErrorException );
123 
125  void SetSickScanFreqAndRes( const sick_lms_1xx_scan_freq_t scan_freq,
126  const sick_lms_1xx_scan_res_t scan_res ) throw( SickTimeoutException, SickIOException, SickErrorException );
127 
129  sick_lms_1xx_scan_freq_t GetSickScanFreq( ) const throw ( SickIOException );
130 
132  sick_lms_1xx_scan_res_t GetSickScanRes( ) const throw ( SickIOException );
133 
135  double GetSickStartAngle( ) const throw ( SickIOException );
136 
138  double GetSickStopAngle( ) const throw ( SickIOException );
139 
141  void SetSickScanDataFormat( const sick_lms_1xx_scan_format_t scan_format ) throw( SickTimeoutException, SickIOException, SickThreadException, SickErrorException );
142 
144  void GetSickMeasurements( unsigned int * const range_1_vals,
145  unsigned int * const range_2_vals,
146  unsigned int * const reflect_1_vals,
147  unsigned int * const reflect_2_vals,
148  unsigned int & num_measurements,
149  unsigned int * const dev_status = NULL ) throw ( SickIOException, SickConfigException, SickTimeoutException );
150 
152  void Uninitialize( const bool disp_banner = true ) throw( SickIOException, SickTimeoutException, SickErrorException, SickThreadException );
153 
155  sick_lms_1xx_scan_freq_t IntToSickScanFreq( const int scan_freq ) const;
156 
158  int SickScanFreqToInt( const sick_lms_1xx_scan_freq_t scan_freq ) const;
159 
161  sick_lms_1xx_scan_res_t DoubleToSickScanRes( const double scan_res ) const;
162 
164  double SickScanResToDouble( const sick_lms_1xx_scan_res_t scan_res ) const;
165 
167  ~SickLMS1xx();
168 
169  private:
170 
184  int32_t sick_stop_angle;
186 
188  std::string _sick_ip_address;
189 
191  uint16_t _sick_tcp_port;
192 
194  struct sockaddr_in _sick_inet_address_info;
195 
198 
201 
204 
207 
210 
213 
215  void _reinitialize( ) throw( SickIOException, SickThreadException, SickTimeoutException, SickErrorException );
216 
218  void _teardownConnection( ) throw( SickIOException );
219 
221  void _updateSickStatus( ) throw( SickTimeoutException, SickIOException );
222 
224  void _getSickScanConfig( ) throw( SickTimeoutException, SickIOException );
225 
227  void _setSickScanConfig( const sick_lms_1xx_scan_freq_t scan_freq,
228  const sick_lms_1xx_scan_res_t scan_res,
229  const int start_angle, const int stop_angle ) throw( SickTimeoutException, SickIOException, SickErrorException );
230 
232  void _setAuthorizedClientAccessMode( ) throw( SickTimeoutException, SickErrorException, SickIOException );
233 
235  void _writeToEEPROM( ) throw( SickTimeoutException, SickIOException );
236 
238  void _sendMessage( const SickLMS1xxMessage &send_message ) const throw ( SickIOException );
239 
241  void _sendMessageAndGetReply( const SickLMS1xxMessage &send_message,
242  SickLMS1xxMessage &recv_message,
243  const std::string reply_command_code,
244  const std::string reply_command,
245  const unsigned int timeout_value = DEFAULT_SICK_LMS_1XX_MESSAGE_TIMEOUT,
246  const unsigned int num_tries = 1 ) throw( SickIOException, SickTimeoutException );
247 
249  void _recvMessage( SickLMS1xxMessage &sick_message ) const throw ( SickTimeoutException );
250 
252  void _startMeasuring( ) throw ( SickTimeoutException, SickIOException );
253 
255  void _stopMeasuring( ) throw ( SickTimeoutException, SickIOException );
256 
258  void _requestDataStream( ) throw ( SickTimeoutException, SickConfigException, SickIOException );
259 
261  void _startStreamingMeasurements( )throw( SickTimeoutException, SickIOException );
262 
264  void _stopStreamingMeasurements( const bool disp_banner = true ) throw( SickTimeoutException, SickIOException );
265 
267  void _checkForMeasuringStatus( unsigned int timeout_value = DEFAULT_SICK_LMS_1XX_STATUS_TIMEOUT ) throw( SickTimeoutException, SickIOException );
268 
270  void _setSickScanDataFormat( const sick_lms_1xx_scan_format_t scan_format ) throw( SickTimeoutException, SickIOException, SickThreadException, SickErrorException );
271 
273  void _restoreMeasuringMode( ) throw( SickTimeoutException, SickIOException );
274 
276  bool _validScanArea( const int start_angle, const int stop_angle ) const;
277 
279  sick_lms_1xx_status_t _intToSickStatus( const int status ) const;
280 
282  void _printSickScanConfig( ) const;
283 
285  void _printInitFooter( ) const;
286 
288  std::string _sickScanDataFormatToString( const sick_lms_1xx_scan_format_t scan_format ) const;
289 
291  double _convertSickAngleUnitsToDegs( const int sick_angle ) const { return ((double)sick_angle)/10000; }
292 
294  unsigned int _convertSickFreqUnitsToHz( const unsigned int sick_freq ) const { return (unsigned int)(((double)sick_freq)/100); }
295 
297  std::string _intToSickConfigErrorStr( const int error ) const;
298 
300  bool _findSubString( const char * const str, const char * const substr, const unsigned int str_length, const unsigned int substr_length,
301  unsigned int &substr_pos, unsigned int start_pos = 0 ) const;
302 
304  char * _convertNextTokenToUInt( char * const str_buffer, unsigned int & num_val, const char * const delimeter = " " ) const;
305 
306  };
307 
313 
319 
325 
331 
332 
333 } //namespace SickToolbox
334 
335 #endif /* SICK_LMS_1XX_HH */
void _getSickScanConfig()
Get the scan configuration of the Sick LMS 1xx.
Definition: SickLMS1xx.cc:918
sick_lms_1xx_status_t _intToSickStatus(const int status) const
Converts int to status.
Definition: SickLMS1xx.cc:2139
sick_lms_1xx_status_t
Defines the Sick LMS 1xx status. This enum lists all of the Sick LMS 1xx status.
Definition: SickLMS1xx.hh:61
double GetSickStopAngle() const
Gets the Sick LMS 1xx scan area start angle [-45,270] deg.
Definition: SickLMS1xx.cc:246
sick_lms_1xx_scan_res_t GetSickScanRes() const
Gets the Sick LMS 1xx scan resolution.
Definition: SickLMS1xx.cc:216
void GetSickMeasurements(unsigned int *const range_1_vals, unsigned int *const range_2_vals, unsigned int *const reflect_1_vals, unsigned int *const reflect_2_vals, unsigned int &num_measurements, unsigned int *const dev_status=NULL)
Acquire multi-pulse sick range measurements.
Definition: SickLMS1xx.cc:316
Thrown when Sick returns an error code or an unexpected response.
void _startMeasuring()
Tell the device to start measuring.
Definition: SickLMS1xx.cc:1404
Contains some simple exception classes.
void _checkForMeasuringStatus(unsigned int timeout_value=DEFAULT_SICK_LMS_1XX_STATUS_TIMEOUT)
Attempts to set and waits until device has in measuring status.
Definition: SickLMS1xx.cc:1730
void _sendMessageAndGetReply(const SickLMS1xxMessage &send_message, SickLMS1xxMessage &recv_message, const std::string reply_command_code, const std::string reply_command, const unsigned int timeout_value=DEFAULT_SICK_LMS_1XX_MESSAGE_TIMEOUT, const unsigned int num_tries=1)
Sends a message and searches for the reply with given command type and command.
Definition: SickLMS1xx.cc:2072
void _writeToEEPROM()
Login as an authorized client.
Definition: SickLMS1xx.cc:1330
std::string _intToSickConfigErrorStr(const int error) const
Definition: SickLMS1xx.cc:2162
sick_lms_1xx_scan_freq_t GetSickScanFreq() const
Gets the Sick LMS 1xx scan frequency.
Definition: SickLMS1xx.cc:201
char * _convertNextTokenToUInt(char *const str_buffer, unsigned int &num_val, const char *const delimeter=" ") const
Utility function for converting next token into unsigned int.
Definition: SickLMS1xx.cc:2275
void _setSickScanDataFormat(const sick_lms_1xx_scan_format_t scan_format)
Definition: SickLMS1xx.cc:1794
sick_lms_1xx_scan_format_t _sick_scan_format
Definition: SickLMS1xx.hh:200
std::string _sick_ip_address
Definition: SickLMS1xx.hh:188
void _setAuthorizedClientAccessMode()
Login as an authorized client.
Definition: SickLMS1xx.cc:1236
sick_lms_1xx_status_t _sick_device_status
Definition: SickLMS1xx.hh:203
Defines a class for monitoring the receive buffer when interfacing w/ a Sick LMS 1xx laser range find...
void Initialize(const bool disp_banner=true)
Initializes the driver and syncs it with Sick LMS 1xx unit. Uses flash params.
Definition: SickLMS1xx.cc:72
void _setupConnection()
Establish a TCP connection to the unit.
Definition: SickLMS1xx.cc:683
#define DEFAULT_SICK_LMS_1XX_TCP_PORT
Sick LMS 1xx TCP/IP Port.
Definition: SickLMS1xx.hh:22
void _requestDataStream()
Request a data data stream type.
Definition: SickLMS1xx.cc:1549
double SickScanResToDouble(const sick_lms_1xx_scan_res_t scan_res) const
Convert sick_lms_1xx_scan_res_t to corresponding double.
Definition: SickLMS1xx.cc:667
int32_t sick_start_angle
Sick scan area start angle.
Definition: SickLMS1xx.hh:183
A structure for aggregrating the Sick LMS 1xx configuration params.
Definition: SickLMS1xx.hh:180
sick_lms_1xx_scan_freq_t sick_scan_freq
Sick scan frequency.
Definition: SickLMS1xx.hh:181
static const int SICK_LMS_1XX_MAX_NUM_MEASUREMENTS
LMS 1xx max number of measurements.
Definition: SickLMS1xx.hh:54
bool _findSubString(const char *const str, const char *const substr, const unsigned int str_length, const unsigned int substr_length, unsigned int &substr_pos, unsigned int start_pos=0) const
Searches a string for a substring.
Definition: SickLMS1xx.cc:2242
void _updateSickStatus()
Get the status of the Sick LMS 1xx.
Definition: SickLMS1xx.cc:852
void _stopMeasuring()
Tell the device to start measuring.
Definition: SickLMS1xx.cc:1476
void _printInitFooter() const
Prints the initialization footer.
Definition: SickLMS1xx.cc:2195
#define DEFAULT_SICK_LMS_1XX_MESSAGE_TIMEOUT
Max time for reply (usecs)
Definition: SickLMS1xx.hh:24
#define DEFAULT_SICK_LMS_1XX_IP_ADDRESS
Default IP Address.
Definition: SickLMS1xx.hh:21
bool _validScanArea(const int start_angle, const int stop_angle) const
Utility function to ensure valid scan area.
Definition: SickLMS1xx.cc:2013
void _teardownConnection()
Teardown TCP connection to Sick LMS 1xx.
Definition: SickLMS1xx.cc:840
void _printSickScanConfig() const
Prints Sick LMS 1xx scan configuration.
Definition: SickLMS1xx.cc:2182
struct sockaddr_in _sick_inet_address_info
Definition: SickLMS1xx.hh:194
void _recvMessage(SickLMS1xxMessage &sick_message) const
Receive a message.
Definition: SickLMS1xx.cc:2113
Provides an abstract parent for all Sick LIDAR devices.
Definition: SickLIDAR.hh:53
sick_lms_1xx_scan_res_t DoubleToSickScanRes(const double scan_res) const
Convert double to corresponding sick_lms_1xx_scan_res_t.
Definition: SickLMS1xx.cc:651
void Uninitialize(const bool disp_banner=true)
Tear down the connection between the host and the Sick LD.
Definition: SickLMS1xx.cc:539
int32_t sick_stop_angle
Sick scan area stop angle.
Definition: SickLMS1xx.hh:184
int SickScanFreqToInt(const sick_lms_1xx_scan_freq_t scan_freq) const
Convert sick_lms_1xx_scan_freq_t to corresponding integer.
Definition: SickLMS1xx.cc:635
#define DEFAULT_SICK_LMS_1XX_STATUS_TIMEOUT
Max time it should take to change status.
Definition: SickLMS1xx.hh:25
SickLMS1xx(const std::string sick_ip_address=DEFAULT_SICK_LMS_1XX_IP_ADDRESS, const uint16_t sick_tcp_port=DEFAULT_SICK_LMS_1XX_TCP_PORT)
A standard constructor.
Definition: SickLMS1xx.cc:52
Provides a simple driver interface for working with the Sick LD-OEM/Sick LD-LRS long-range models via...
Definition: SickLMS1xx.hh:50
void _stopStreamingMeasurements(const bool disp_banner=true)
Stop Measurment Stream.
Definition: SickLMS1xx.cc:1660
Defines the abstract parent class for defining a Sick LIDAR device driver.
sick_lms_1xx_scan_res_t sick_scan_res
Sick scan resolution.
Definition: SickLMS1xx.hh:182
double GetSickStartAngle() const
Gets the Sick LMS 1xx scan area start angle [-45,270] deg.
Definition: SickLMS1xx.cc:231
sick_lms_1xx_scan_freq_t IntToSickScanFreq(const int scan_freq) const
Convert integer to corresponding sick_lms_1xx_scan_freq_t.
Definition: SickLMS1xx.cc:619
sick_lms_1xx_scan_res_t
Defines the Sick LMS 1xx scan resolution. This enum lists all of the Sick LMS 1xx scan resolutions...
Definition: SickLMS1xx.hh:109
void SetSickScanFreqAndRes(const sick_lms_1xx_scan_freq_t scan_freq, const sick_lms_1xx_scan_res_t scan_res)
Sets the Sick LMS 1xx scan frequency and resolution.
Definition: SickLMS1xx.cc:148
Encapsulates the Sick LIDAR Matlab/C++ toolbox.
Definition: SickLD.cc:44
sick_lms_1xx_scan_freq_t
Defines the Sick LMS 1xx scan frequency. This enum lists all of the Sick LMS 1xx scan frequencies...
Definition: SickLMS1xx.hh:96
double _convertSickAngleUnitsToDegs(const int sick_angle) const
Definition: SickLMS1xx.hh:291
void _setSickScanConfig(const sick_lms_1xx_scan_freq_t scan_freq, const sick_lms_1xx_scan_res_t scan_res, const int start_angle, const int stop_angle)
Set the Sick LMS 1xx scan configuration (volatile, does not write to EEPROM)
Definition: SickLMS1xx.cc:1063
sick_lms_1xx_scan_config_t _sick_scan_config
Definition: SickLMS1xx.hh:197
void SetSickScanDataFormat(const sick_lms_1xx_scan_format_t scan_format)
Definition: SickLMS1xx.cc:260
void _reinitialize()
Re-initializes the Sick LMS 1xx.
Definition: SickLMS1xx.cc:790
Thrown instance where the driver can&#39;t read,write,drain,flush,... the buffers.
sick_lms_1xx_scan_format_t
Defines the Sick LMS 1xx scan types This enum is for specifiying the desired scan returns...
Definition: SickLMS1xx.hh:79
Thrown when the driver detects (or the Sick reports) an invalid config.
Makes handling timeouts much easier.
A class to represent all messages sent to and from the Sick LMS 1xx unit.
void _sendMessage(const SickLMS1xxMessage &send_message) const
Sends a message without waiting for reply.
Definition: SickLMS1xx.cc:2038
unsigned int _convertSickFreqUnitsToHz(const unsigned int sick_freq) const
Definition: SickLMS1xx.hh:294
Thrown when error occurs during thread initialization, and uninitialization.
struct SickToolbox::SickLMS1xx::sick_lms_1xx_scan_config_tag sick_lms_1xx_scan_config_t
Adopt c-style convention.
std::string _sickScanDataFormatToString(const sick_lms_1xx_scan_format_t scan_format) const
Utility function for returning scan format as string.
Definition: SickLMS1xx.cc:2210


sicktoolbox
Author(s): Jason Derenick , Thomas Miller
autogenerated on Tue Sep 10 2019 03:37:34