MRPT  2.0.3
test.cpp
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 
10 #include <mrpt/comms/CSerialPort.h>
15 #include <mrpt/system/CTicTac.h>
16 #include <mrpt/system/os.h>
18 #include <iostream>
19 
20 using namespace mrpt;
21 using namespace mrpt::hwdrivers;
22 using namespace mrpt::obs;
23 using namespace mrpt::maps;
24 using namespace mrpt::gui;
25 using namespace mrpt::poses;
26 using namespace mrpt::system;
27 using namespace std;
28 
29 string SERIAL_NAME; // Name of the serial port to open
30 
31 // ------------------------------------------------------
32 // Test_HOKUYO
33 // ------------------------------------------------------
34 void Test_HOKUYO()
35 {
36  CHokuyoURG laser;
37 
38  string serName, type;
39 
40  string ip;
41 
42  unsigned int port;
43 
44  cout << "Specify the type of the Hokuyo connection, usb or ethernet: ";
45  getline(cin, type);
46 
47  while ((mrpt::system::lowerCase(type) != "usb") &&
48  (mrpt::system::lowerCase(type) != "ethernet"))
49  {
50  cout << "Incorrect type" << endl;
51  cout << "Specify the type of the Hokuyo connection, usb or ethernet: ";
52  getline(cin, type);
53  }
54 
55  cout << endl
56  << endl
57  << "HOKUYO laser range finder test application." << endl
58  << endl;
59 
60  if (mrpt::system::lowerCase(type) == "usb")
61  {
62  if (SERIAL_NAME.empty())
63  {
64  cout << "Enter the serial port name (e.g. COM1, ttyS0, ttyUSB0, "
65  "ttyACM0): ";
66  getline(cin, serName);
67  }
68  else
69  {
70  cout << "Using serial port: " << SERIAL_NAME << endl;
71  serName = SERIAL_NAME;
72  }
73 
74  // Set the laser serial port:
75  laser.setSerialPort(serName);
76  }
77  else
78  {
79  cout << "Enter the ip direction: ";
80  getline(cin, ip);
81 
82  cout << "Enter the port number: ";
83  cin >> port;
84 
85  // Set the laser serial port:
86  laser.setIPandPort(ip, port);
87  }
88  string intensity;
89  cout << endl << endl << "Enable intensity [y/n]:";
90  getline(cin, intensity);
91  laser.setIntensityMode(mrpt::system::lowerCase(intensity) == "y");
92 
93  // Config: Use defaults + selected port ( serial or ethernet )
94 
95  printf("[TEST] Turning laser ON...\n");
96  if (laser.turnOn())
97  printf("[TEST] Initialization OK!\n");
98  else
99  {
100  printf("[TEST] Initialization failed!\n");
101  return;
102  }
103 
104 #if MRPT_HAS_WXWIDGETS
105  CDisplayWindowPlots win("Laser scans");
106 #endif
107 
108  cout << "Press any key to stop capturing..." << endl;
109 
110  CTicTac tictac;
111  tictac.Tic();
112 
113  while (!mrpt::system::os::kbhit())
114  {
115  bool thereIsObservation, hardError;
117 
118  laser.doProcessSimple(thereIsObservation, obs, hardError);
119 
120  if (hardError) printf("[TEST] Hardware error=true!!\n");
121 
122  if (thereIsObservation)
123  {
124  double FPS = 1.0 / tictac.Tac();
125 
126  obs.getDescriptionAsText(std::cout);
127 
128  obs.sensorPose = CPose3D(0, 0, 0);
129 
132  theMap.insertObservation(obs);
133  // map.save2D_to_text_file("_out_scan.txt");
134 
135  /*
136  COpenGLScene scene3D;
137  opengl::CPointCloud::Ptr points =
138  opengl::CPointCloud::Create();
139  points->loadFromPointsMap(&map);
140  scene3D.insert(points);
141  CFileStream("_out_point_cloud.3Dscene",fomWrite) << scene3D;
142  */
143 
144 #if MRPT_HAS_WXWIDGETS
145  std::vector<float> xs, ys, zs;
146  theMap.getAllPoints(xs, ys, zs);
147  win.plot(xs, ys, ".b3");
148  win.axis_equal();
149 #endif
150 
151  tictac.Tic();
152  }
153 
154  std::this_thread::sleep_for(15ms);
155  };
156 
157  laser.turnOff();
158 }
159 
160 int main(int argc, char** argv)
161 {
162  try
163  {
164  if (argc > 1)
165  {
166  SERIAL_NAME = string(argv[1]);
167  }
168 
169  Test_HOKUYO();
170  return 0;
171  }
172  catch (const std::exception& e)
173  {
174  std::cerr << "MRPT error: " << mrpt::exception_to_str(e) << std::endl;
175  return -1;
176  }
177  catch (...)
178  {
179  printf("Another exception!!");
180  return -1;
181  }
182 }
mrpt::system::os::kbhit
bool kbhit() noexcept
An OS-independent version of kbhit, which returns true if a key has been pushed.
Definition: os.cpp:392
os.h
SERIAL_NAME
string SERIAL_NAME
Definition: vision_stereo_rectify/test.cpp:25
string_utils.h
mrpt::system::CTicTac
A high-performance stopwatch, with typical resolution of nanoseconds.
Definition: system/CTicTac.h:17
mrpt::obs::CObservation2DRangeScan
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
Definition: CObservation2DRangeScan.h:54
mrpt::maps::CMetricMap::insertObservation
bool insertObservation(const mrpt::obs::CObservation &obs, const mrpt::poses::CPose3D *robotPose=nullptr)
Insert the observation information into this map.
Definition: CMetricMap.cpp:93
mrpt::maps::CPointsMap::TInsertionOptions::minDistBetweenLaserPoints
float minDistBetweenLaserPoints
The minimum distance between points (in 3D): If two points are too close, one of them is not inserted...
Definition: CPointsMap.h:235
CSerialPort.h
mrpt::hwdrivers
Contains classes for various device interfaces.
Definition: C2DRangeFinderAbstract.h:19
CHokuyoURG.h
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: BaseAppDataSource.h:15
mrpt::maps::CPointsMap::getAllPoints
void getAllPoints(VECTOR &xs, VECTOR &ys, VECTOR &zs, size_t decimation=1) const
Returns a copy of the 2D/3D points as a std::vector of float coordinates.
Definition: CPointsMap.h:590
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::poses
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CHierarchicalMapMHPartition.h:22
mrpt::obs
This namespace contains representation of robot actions and observations.
Definition: CParticleFilter.h:17
mrpt::obs::CObservation2DRangeScan::getDescriptionAsText
void getDescriptionAsText(std::ostream &o) const override
Build a detailed, multi-line textual description of the observation contents and dump it to the outpu...
Definition: CObservation2DRangeScan.cpp:453
mrpt::system::lowerCase
std::string lowerCase(const std::string &str)
Returns an lower-case version of a string.
Definition: string_utils.cpp:26
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::system::CTicTac::Tac
double Tac() noexcept
Stops the stopwatch.
Definition: CTicTac.cpp:86
main
int main()
Definition: vision_stereo_rectify/test.cpp:78
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
win
mrpt::gui::CDisplayWindow3D::Ptr win
Definition: vision_stereo_rectify/test.cpp:31
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::setIntensityMode
bool setIntensityMode(bool enabled)
If true scans will capture intensity.
Definition: CHokuyoURG.cpp:733
mrpt::maps::CSimplePointsMap
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans.
Definition: CSimplePointsMap.h:30
argv
const char * argv[]
Definition: RawlogGrabberApp_unittest.cpp:40
mrpt::system::CTicTac::Tic
void Tic() noexcept
Starts the stopwatch.
Definition: CTicTac.cpp:75
mrpt::gui
Classes for creating GUI windows for 2D and 3D visualization.
Definition: about_box.h:14
mrpt::gui::CDisplayWindowPlots
Create a GUI window and display plots with MATLAB-like interfaces and commands.
Definition: CDisplayWindowPlots.h:33
Test_HOKUYO
void Test_HOKUYO()
Definition: vision_stereo_rectify/test.cpp:34
mrpt::maps::CPointsMap::insertionOptions
TInsertionOptions insertionOptions
The options used when inserting observations in the map.
Definition: CPointsMap.h:274
CTicTac.h
argc
const int argc
Definition: RawlogGrabberApp_unittest.cpp:41
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::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::maps
Definition: CBeacon.h:21
mrpt::exception_to_str
std::string exception_to_str(const std::exception &e)
Builds a nice textual representation of a nested exception, which if generated using MRPT macros (THR...
Definition: exceptions.cpp:59
CDisplayWindowPlots.h
CSimplePointsMap.h
mrpt::obs::CObservation2DRangeScan::sensorPose
mrpt::poses::CPose3D sensorPose
The 6D pose of the sensor on the robot at the moment of starting the scan.
Definition: CObservation2DRangeScan.h:122
CObservation2DRangeScan.h
mrpt::system
Definition: backtrace.h:14



Page generated by Doxygen 1.8.17 for MRPT 2.0.3 at Fri May 15 23:51:15 UTC 2020