1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
|
/* +------------------------------------------------------------------------+
| Mobile Robot Programming Toolkit (MRPT) |
| https://www.mrpt.org/ |
| |
| Copyright (c) 2005-2023, Individual contributors, see AUTHORS file |
| See: https://www.mrpt.org/Authors - All rights reserved. |
| Released under BSD License. See: https://www.mrpt.org/License |
+------------------------------------------------------------------------+ */
#include <mrpt/hwdrivers/CRoboPeakLidar.h>
#include <mrpt/obs/CObservation2DRangeScan.h>
#include <mrpt/system/CTicTac.h>
#include <mrpt/system/os.h>
#include <mrpt/system/string_utils.h>
#include <chrono>
#include <iostream>
#include <thread>
using namespace mrpt;
using namespace mrpt::hwdrivers;
using namespace mrpt::obs;
using namespace mrpt::gui;
using namespace mrpt::system;
using namespace std;
string SERIAL_NAME; // Name of the serial port to open
// ------------------------------------------------------
// Test_RPLIDAR
// ------------------------------------------------------
void Test_RPLIDAR()
{
CRoboPeakLidar laser;
string serName;
if (SERIAL_NAME.empty())
{
std::cout << "Enter the serial port name (e.g. COM1, ttyS0, ttyUSB0, "
"ttyACM0): ";
getline(cin, serName);
}
else
{
std::cout << "Using serial port: " << SERIAL_NAME << endl;
serName = SERIAL_NAME;
}
// Set the laser serial port:
laser.setSerialPort(serName);
// Show GUI preview:
laser.showPreview(true);
// Config: Use defaults + selected port ( serial or ethernet )
printf("Turning laser ON...\n");
if (laser.turnOn()) printf("Initialization OK!\n");
else
{
printf("Initialization failed!\n");
return;
}
cout << "Press any key to stop capturing..." << endl;
CTicTac tictac;
tictac.Tic();
while (!mrpt::system::os::kbhit())
{
bool thereIsObservation, hardError;
CObservation2DRangeScan obs;
laser.doProcessSimple(thereIsObservation, obs, hardError);
if (hardError) printf("[TEST] Hardware error=true!!\n");
if (thereIsObservation)
{
double FPS = 1.0 / tictac.Tac();
printf(
"Scan received: %u ranges, FOV: %.02fdeg, %.03fHz: mid "
"rang=%fm\n",
(unsigned int)obs.getScanSize(), RAD2DEG(obs.aperture), FPS,
obs.getScanRange(obs.getScanSize() / 2));
obs.sensorPose = mrpt::poses::CPose3D(0, 0, 0);
tictac.Tic();
}
std::this_thread::sleep_for(5ms);
};
}
int main(int argc, char** argv)
{
try
{
if (argc > 1) SERIAL_NAME = string(argv[1]);
Test_RPLIDAR();
return 0;
}
catch (const std::exception& e)
{
std::cerr << "MRPT error: " << mrpt::exception_to_str(e) << std::endl;
return -1;
}
catch (...)
{
printf("Another exception!!");
return -1;
}
}
|
