lms1xx/lms1xx_simple_app/src/main.cc
Go to the documentation of this file.
1 
16 #include <stdlib.h>
17 #include <string>
18 #include <iostream>
20 
21 using namespace std;
22 using namespace SickToolbox;
23 
24 int main(int argc, char* argv[])
25 {
26 
27  /*
28  * Instantiate an instance
29  */
30  SickLMS1xx sick_lms_1xx;
31 
32  /*
33  * Initialize the Sick LMS 2xx
34  */
35  try {
36  sick_lms_1xx.Initialize();
37  }
38 
39  catch(...) {
40  cerr << "Initialize failed! Are you using the correct IP address?" << endl;
41  return -1;
42  }
43 
44  try {
45  unsigned int status = 1;
46  unsigned int num_measurements = 0;
47  unsigned int range_1_vals[SickLMS1xx::SICK_LMS_1XX_MAX_NUM_MEASUREMENTS];
48  unsigned int range_2_vals[SickLMS1xx::SICK_LMS_1XX_MAX_NUM_MEASUREMENTS];
49  //sick_lms_1xx.SetSickScanFreqAndRes(SickLMS1xx::SICK_LMS_1XX_SCAN_FREQ_25,
50  //SickLMS1xx::SICK_LMS_1XX_SCAN_RES_25);
51  //sick_lms_1xx.SetSickScanDataFormat(SickLMS1xx::SICK_LMS_1XX_DIST_DOUBLE_PULSE,
52  // SickLMS1xx::SICK_LMS_1XX_REFLECT_NONE);
53  sick_lms_1xx.SetSickScanDataFormat(SickLMS1xx::SICK_LMS_1XX_SCAN_FORMAT_DIST_DOUBLE_PULSE_REFLECT_16BIT);
54  for (int i = 0; i < 1000; i++) {
55  sick_lms_1xx.GetSickMeasurements(range_1_vals,range_2_vals,range_1_vals,range_2_vals,num_measurements,&status);
56  std::cout << i << ": " << num_measurements << " " << status << std::endl;
57  }
58  }
59 
60  catch(SickConfigException sick_exception) {
61  std::cout << sick_exception.what() << std::endl;
62  }
63 
64  catch(SickIOException sick_exception) {
65  std::cout << sick_exception.what() << std::endl;
66  }
67 
68  catch(SickTimeoutException sick_exception) {
69  std::cout << sick_exception.what() << std::endl;
70  }
71 
72  catch(...) {
73  cerr << "An Error Occurred!" << endl;
74  return -1;
75  }
76 
77 
78  /*
79  * Uninitialize the device
80  */
81  try {
82  sick_lms_1xx.Uninitialize();
83  }
84 
85  catch(...) {
86  cerr << "Uninitialize failed!" << endl;
87  return -1;
88  }
89 
90  /* Success! */
91  return 0;
92 
93 }
94 
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
int main(int argc, char *argv[])
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
virtual const char * what() const
From the standard exception library.
void Uninitialize(const bool disp_banner=true)
Tear down the connection between the host and the Sick LD.
Definition: SickLMS1xx.cc:539
Provides a simple driver interface for working with the Sick LD-OEM/Sick LD-LRS long-range models via...
Definition: SickLMS1xx.hh:50
Encapsulates the Sick LIDAR Matlab/C++ toolbox.
Definition: SickLD.cc:44
void SetSickScanDataFormat(const sick_lms_1xx_scan_format_t scan_format)
Definition: SickLMS1xx.cc:260
Defines the SickLMS1xx class for working with the Sick LMS1xx laser range finders.
Thrown instance where the driver can&#39;t read,write,drain,flush,... the buffers.
Thrown when the driver detects (or the Sick reports) an invalid config.
Makes handling timeouts much easier.


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