-
Notifications
You must be signed in to change notification settings - Fork 2
/
Lidar.cpp
89 lines (77 loc) · 2.05 KB
/
Lidar.cpp
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
/*
* Lidar.cpp
*
* Created on: 16.12.2018
* Author: oliver
*/
#include "Lidar.h"
#include <stdio.h>
#include <iostream>
#include <thread>
#include <chrono>
namespace lidar {
Lidar::Lidar() {
std::cout << "Init Lidar..." << std::endl;
/*
* Baudrate for ydlidar X4 is always 128000
* Note: the baudrate must be altered for
* other types.
*/
baudrate = 128000;
std::string intensity;
ydlidar::init(0, nullptr);
std::map<std::string, std::string> ports = ydlidar::YDlidarDriver::lidarPortList();
std::map<std::string,std::string>::iterator it;
if(ports.size()==1) {
it = ports.begin();
port = it->second;
} else {
std::cerr << "Did not detect YDLidar port!" << std::endl;
}
std::cout << "Port: " << port << std::endl;
cylidar.setSerialPort(port);
cylidar.setSerialBaudrate(baudrate);
cylidar.setIntensities(false);
cylidar.setAutoReconnect(true);//hot plug
cylidar.setReversion(false);
cylidar.setFixedResolution(false);
cylidar.setScanFrequency(8);
cylidar.initialize();
}
Lidar::~Lidar() {}
Lidar& Lidar::getInstance()
{
static Lidar instance;
return instance;
}
bool Lidar::isReady()
{
return ydlidar::ok();
}
std::vector<std::tuple<float, float>> Lidar::scan() {
/*
* Assuming a resolution of 0.5 degree at
* standard scan speed of 8 Hz
*/
static node_info nodes[720];
static size_t count = _countof(nodes);
std::vector<std::tuple<float, float>> scanData;
// wait Scan data:
//uint64_t tim_scan_start = getCurrentTime();
while (!IS_OK(cylidar.lidarPtr->grabScanData(nodes, count))) {
std::cout << "sleep" << std::endl;
std::this_thread::sleep_for(std::chrono::milliseconds(10));
};
//uint64_t tim_scan_end = getCurrentTime();
auto op_result = cylidar.lidarPtr->ascendScanData(nodes, count);
if (IS_OK(op_result)) {
for (unsigned int i = 0; i < count; i++) {
if (nodes[i].distance_q2 != 0) {
float angle = (float)((nodes[i].angle_q6_checkbit >> LIDAR_RESP_MEASUREMENT_ANGLE_SHIFT) / 64.0f);
scanData.push_back(std::make_tuple(angle, nodes[i].distance_q2 / 4000.0f));
}
}
}
return scanData;
}
}