-
Notifications
You must be signed in to change notification settings - Fork 35
/
LidarServer.java
executable file
·168 lines (149 loc) · 5.3 KB
/
LidarServer.java
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
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
package com.team254.frc2018.lidar;
import com.team254.frc2018.Constants;
import edu.wpi.first.wpilibj.Timer;
import java.io.BufferedReader;
import java.io.EOFException;
import java.io.IOException;
import java.io.InputStreamReader;
/**
* Starts the <code>chezy_lidar</code> C++ program, parses its
* output, and feeds the LIDAR points to the {@link LidarProcessor}.
* <p>
* Once started, a separate thread reads the stdout of the
* <code>chezy_lidar</code> process and parses the (angle, distance)
* values in each line. Each resulting {@link LidarPoint} is passed
* to {@link LidarProcessor.addPoint(...)}.
*/
public class LidarServer {
private static LidarServer mInstance = null;
private final LidarProcessor mLidarProcessor = LidarProcessor.getInstance();
private static BufferedReader mBufferedReader;
private boolean mRunning = false;
private Thread mThread;
private Process mProcess;
private boolean mEnding = false;
public static LidarServer getInstance() {
if (mInstance == null) {
mInstance = new LidarServer();
}
return mInstance;
}
public boolean isLidarConnected() {
try {
Runtime r = Runtime.getRuntime();
Process p = r.exec("/bin/ls /dev/serial/by-id/");
InputStreamReader reader = new InputStreamReader(p.getInputStream());
BufferedReader response = new BufferedReader(reader);
String s;
while ((s = response.readLine()) != null) {
if (s.equals("usb-Silicon_Labs_CP2102_USB_to_UART_Bridge_Controller_0001-if00-port0"))
return true;
}
} catch (IOException e) {
e.printStackTrace();
}
return false;
}
public boolean start() {
if (!isLidarConnected()) {
System.err.println("Cannot start LidarServer: not connected");
return false;
}
synchronized (this) {
if (mRunning) {
System.err.println("Cannot start LidarServer: already running");
return false;
}
if (mEnding) {
System.err.println("Cannot start LidarServer: thread ending");
return false;
}
mRunning = true;
}
System.out.println("Starting lidar");
try {
mProcess = new ProcessBuilder().command(Constants.kChezyLidarPath).start();
mThread = new Thread(new ReaderThread());
mThread.start();
InputStreamReader reader = new InputStreamReader(mProcess.getInputStream());
mBufferedReader = new BufferedReader(reader);
} catch (Exception e) {
e.printStackTrace();
}
return true;
}
public boolean stop() {
synchronized (this) {
if (!mRunning) {
System.err.println("Cannot stop LidarServer: not running");
return false;
}
mRunning = false;
mEnding = true;
}
System.out.println("Stopping Lidar...");
try {
mProcess.destroyForcibly();
mProcess.waitFor();
mThread.join();
} catch (InterruptedException e) {
System.err.println("Error: Interrupted while stopping lidar");
e.printStackTrace();
synchronized (this) {
mEnding = false;
}
return false;
}
System.out.println("Lidar Stopped");
synchronized (this) {
mEnding = false;
}
return true;
}
public synchronized boolean isRunning() {
return mRunning;
}
public synchronized boolean isEnding() {
return mEnding;
}
private void handleLine(String line) {
boolean isNewScan = line.substring(line.length() - 1).equals("s");
if (isNewScan) {
line = line.substring(0, line.length() - 1);
}
long curSystemTime = System.currentTimeMillis();
double curFPGATime = Timer.getFPGATimestamp();
String[] parts = line.split(",");
if (parts.length == 3) {
try {
long ts = Long.parseLong(parts[0]);
long ms_ago = curSystemTime - ts;
double normalizedTs = curFPGATime - (ms_ago / 1000.0f);
double angle = Double.parseDouble(parts[1]);
double distance = Double.parseDouble(parts[2]);
if (distance != 0)
mLidarProcessor.addPoint(new LidarPoint(normalizedTs, angle, distance), isNewScan);
} catch (java.lang.NumberFormatException e) {
e.printStackTrace();
}
}
}
private class ReaderThread implements Runnable {
@Override
public void run() {
while (isRunning()) {
try {
if (mBufferedReader.ready()) {
String line = mBufferedReader.readLine();
if (line == null) { // EOF
throw new EOFException("End of chezy-lidar process InputStream");
}
handleLine(line);
}
} catch (IOException e) {
e.printStackTrace();
}
}
}
}
}