Skip to content

Commit fdb29e9

Browse files
author
RogerKSI
committed
update object avoidance
1 parent 758595b commit fdb29e9

File tree

4 files changed

+166
-55
lines changed

4 files changed

+166
-55
lines changed

‎_main.cpp

Lines changed: 11 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -29,7 +29,7 @@ using namespace cv;
2929

3030
#define Create_Comport "COM3"
3131
#define PORT_NUM 1500
32-
#define IP_SERVER "127.0.0.1"
32+
#define IP_SERVER "192.168.43.85"
3333
#define RADIUS 150
3434

3535
bool isRecord = false;
@@ -143,20 +143,24 @@ int main()
143143
if (mouseClick) {
144144
circle(matClick, Point(-1 * mouseZ + RADIUS, -1 * mouseX + RADIUS), 40, Scalar(0, 0, 0), -1);
145145

146-
int vx = (int)(mouseX * 10.0 / (RADIUS + 1));
147-
int vz = (int)(mouseZ * 10.0 / (RADIUS + 1));
148-
int sum = (vx + vz) > 10 ? (vx + vz) : 10;
149-
vx = (int)(vx * 10.0 / sum);
150-
vz = (int)(vz * 10.0 / sum);
146+
double tempx = (1.0 * mouseX / (RADIUS + 1));
147+
double tempz = (1.0 * mouseZ / (RADIUS + 1));
148+
double length = sqrt(tempx*tempx + tempz*tempz);
149+
150+
int vx = (int)(tempx * 10.0 * length);
151+
int vz = (int)(tempz * 10.0 * length);
152+
151153
string str_send = "";
152154
str_send += vx >= 0 ? "+" : "-";
153155
str_send += to_string(abs(vx));
154156
str_send += "|";
157+
if (vx < 0)
158+
vz *= -1;
155159
str_send += vz >= 0 ? "+" : "-";
156160
str_send += to_string(abs(vz));
157161
str_send += '\0';
158162

159-
cout << "send: " << str_send << endl;
163+
// cout << "send: " << str_send << "=> mean: " << vx - vz << "|" << vx + vz << endl;
160164
send(client, str_send.c_str(), str_send.size(), 0);
161165
}
162166

‎_main_find_depth_at_point.cpp

Lines changed: 20 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -24,7 +24,7 @@ void CallBackFunc(int event, int x, int y, int flags, void* userdata)
2424
if (event == EVENT_LBUTTONDOWN)
2525
{
2626
cout << "Left button of the mouse is clicked - position (" << x << ", " << y << ")" << endl;
27-
cout << depthImg.at<USHORT>(y / 2, x / 2)<<endl;
27+
cout << depthImg.at<USHORT>(y / 2, x / 2) << endl;
2828
}
2929
}
3030

@@ -35,10 +35,27 @@ int main() {
3535
while (true) {
3636

3737
kin.GrabData(depthImg, colorImg, indexImg, pointImg);
38-
38+
3939
setMouseCallback("colorImg", CallBackFunc, NULL);
4040

41-
imshow("depthImg", depthImg);
41+
for (int i = 0; i<1280; i++) {
42+
for (int j = 0; j < 960; j++) {
43+
if (depthImg.at<USHORT>(j / 2, i / 2) == 0)
44+
colorImg.at<Vec3b>(j, i) = Vec3b(0, 0, 255);
45+
else if (depthImg.at<USHORT>(j / 2, i / 2) <= 600)
46+
colorImg.at<Vec3b>(j, i) = Vec3b(255, 0, 0);
47+
else if (depthImg.at<USHORT>(j / 2, i / 2) <= 800)
48+
colorImg.at<Vec3b>(j, i) = Vec3b(0, 255, 0);
49+
50+
if (j == 480) {
51+
if (i >= 540 && i <= 740) {
52+
colorImg.at<Vec3b>(j, i) = Vec3b(0, 0, 0);
53+
}
54+
}
55+
}
56+
}
57+
58+
//imshow("depthImg", depthImg);
4259
imshow("colorImg", colorImg);
4360

4461
waitKey(100);

‎_main_tcp_client(person).cpp

Lines changed: 11 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -29,7 +29,7 @@ using namespace cv;
2929

3030
#define Create_Comport "COM3"
3131
#define PORT_NUM 1500
32-
#define IP_SERVER "127.0.0.1"
32+
#define IP_SERVER "192.168.43.85"
3333
#define RADIUS 150
3434

3535
bool isRecord = false;
@@ -143,20 +143,24 @@ int main()
143143
if (mouseClick) {
144144
circle(matClick, Point(-1 * mouseZ + RADIUS, -1 * mouseX + RADIUS), 40, Scalar(0, 0, 0), -1);
145145

146-
int vx = (int)(mouseX * 10.0 / (RADIUS + 1));
147-
int vz = (int)(mouseZ * 10.0 / (RADIUS + 1));
148-
int sum = (vx + vz) > 10 ? (vx + vz) : 10;
149-
vx = (int)(vx * 10.0 / sum);
150-
vz = (int)(vz * 10.0 / sum);
146+
double tempx = (1.0 * mouseX / (RADIUS + 1));
147+
double tempz = (1.0 * mouseZ / (RADIUS + 1));
148+
double length = sqrt(tempx*tempx + tempz*tempz);
149+
150+
int vx = (int)(tempx * 10.0 * length);
151+
int vz = (int)(tempz * 10.0 * length);
152+
151153
string str_send = "";
152154
str_send += vx >= 0 ? "+" : "-";
153155
str_send += to_string(abs(vx));
154156
str_send += "|";
157+
if (vx < 0)
158+
vz *= -1;
155159
str_send += vz >= 0 ? "+" : "-";
156160
str_send += to_string(abs(vz));
157161
str_send += '\0';
158162

159-
cout << "send: " << str_send << endl;
163+
// cout << "send: " << str_send << "=> mean: " << vx - vz << "|" << vx + vz << endl;
160164
send(client, str_send.c_str(), str_send.size(), 0);
161165
}
162166

‎_main_tcp_server(robot).cpp

Lines changed: 124 additions & 38 deletions
Original file line numberDiff line numberDiff line change
@@ -38,15 +38,17 @@ DWORD WINAPI streamVideo(LPVOID lpParameter)
3838
{
3939
int& server = *((int*)lpParameter);
4040

41-
VideoCapture cap(1);
41+
//VideoCapture cap(1);
4242

4343
while (true) {
4444
Mat src;
4545
Mat dst;
4646
Size size(160, 120);
4747

48-
cap >> src;
49-
resize(src, dst, size);
48+
//cap >> src;
49+
//resize(src, dst, size);
50+
51+
resize(colorImg, dst, size);
5052

5153
int dstSize = dst.total() * dst.elemSize();
5254

@@ -63,24 +65,93 @@ DWORD WINAPI streamVideo(LPVOID lpParameter)
6365
}
6466

6567
int check_wall(Mat depthImg) {
66-
int x_wall = 200;
68+
/*int x_wall = 250;
6769
int y_wall = 2;
6870
int count = 0;
6971
for (int i = 320 - x_wall / 2; i < 320 + x_wall / 2; i++) {
70-
for (int j = 300 - y_wall / 2; j < 300 + y_wall / 2; j++) {
71-
int depth = depthImg.at<USHORT>(j, i);
72-
73-
if (depth == 0)
74-
count++;
75-
else if (depth < 800 && depth > 600)
76-
return 1;
77-
else if (depth <= 600)
78-
return 2;
72+
for (int j = 240 - y_wall / 2; j < 240 + y_wall / 2; j++) {
73+
int depth = depthImg.at<USHORT>(j, i);
74+
75+
if (depth == 0)
76+
count++;
77+
else if (depth < 800 && depth > 600)
78+
return 1;
79+
else if (depth <= 600)
80+
return 2;
81+
}
82+
}
83+
if (count >= 170)
84+
return 3;
85+
return 0;
86+
*/
87+
int countleft = 0;
88+
int typeleft = 0;
89+
int xleft = 0;
90+
int flagleft = 0;
91+
for (int i = 0; i < 320; i++) {
92+
int depth = depthImg.at<USHORT>(240, i);
93+
94+
int x = (int)(depth * (i - 320) / 5240);
95+
if (depth == 0)
96+
countleft++;
97+
else if (depth < 800 && depth > 600) {
98+
typeleft = 1;
99+
xleft = x;
100+
}
101+
else if (depth <= 600) {
102+
typeleft = 2;
103+
xleft = x;
104+
}
105+
else {
106+
if (x >= -20) {
107+
flagleft = 1;
108+
}
109+
}
110+
}
111+
112+
113+
int countright = 0;
114+
int typeright = 0;
115+
int xright = 0;
116+
int flagright = 0;
117+
for (int i = 639; i >= 320; i--) {
118+
int depth = depthImg.at<USHORT>(240, i);
119+
120+
int x = (int)(depth * (i - 320) / 5240);
121+
if (depth == 0)
122+
countright++;
123+
else if (depth < 800 && depth > 600) {
124+
typeright = 1;
125+
xright = x;
126+
}
127+
else if (depth <= 600) {
128+
typeright = 2;
129+
xright = x;
79130
}
131+
else {
132+
if (x <= 20) {
133+
flagright = 1;
134+
}
135+
}
136+
}
137+
138+
if (typeleft == 1 || typeleft == 2) {
139+
if (xleft >= -20)
140+
return typeleft;
141+
}
142+
143+
else if (typeright == 1 || typeright == 2) {
144+
if (xright <= 20)
145+
return typeright;
146+
}
147+
else if (flagleft == 1 && flagright == 1)
148+
return 0;
149+
else {
150+
if (countleft > 250 || countright > 250)
151+
return 2;
80152
}
81-
if (count == 400)
82-
return 3;
83153
return 0;
154+
84155
}
85156

86157
int main()
@@ -146,16 +217,17 @@ int main()
146217
cout << "=> Error on accepting..." << endl;
147218
continue;
148219
}
220+
221+
DWORD timeout = 100;
222+
setsockopt(server, SOL_SOCKET, SO_RCVTIMEO, (char*)&timeout, sizeof(timeout));
223+
149224
}
150225

151226
//////////////////////////////////////////////
152227
// video streaming
153228
//////////////////////////////////////////////
154229

155230
if (mode == 's' || mode == 'c') {
156-
DWORD myThreadID;
157-
HANDLE myHandle = CreateThread(0, 0, streamVideo, &server, 0, &myThreadID);
158-
159231
strcpy(buffer, "=> Server connected...\n");
160232
send(server, buffer, bufsize, 0);
161233
}
@@ -167,11 +239,9 @@ int main()
167239
if (mode == 'c') {
168240

169241
while (true) {
170-
cout << "receive: ";
171242
if ((nReadBytes = recv(server, buffer, bufsize, 0)) == SOCKET_ERROR)
172243
break;
173244

174-
cout << buffer << endl;
175245
}
176246
if (nReadBytes == SOCKET_ERROR) {
177247
closesocket(server);
@@ -185,6 +255,7 @@ int main()
185255
RobotConnector robot;
186256
KinectConnector kin = KinectConnector();
187257

258+
188259
if (!robot.Connect(Create_Comport) || !kin.Connect()) {
189260
cout << "Error : Can't connect to robot or kinect" << endl;
190261

@@ -195,39 +266,52 @@ int main()
195266
continue;
196267
}
197268

269+
kin.GrabData(depthImg, colorImg, indexImg, pointImg);
270+
271+
if (mode == 's' || mode == 'c') {
272+
DWORD myThreadID;
273+
HANDLE myHandle = CreateThread(0, 0, streamVideo, &server, 0, &myThreadID);
274+
}
275+
198276
robot.DriveDirect(0, 0);
199277
cvNamedWindow("Robot");
200-
278+
279+
int counter = 0;
201280
while (true)
202281
{
203282

204283
double vx, vz;
205-
vx = vz = 0.0;
284+
if(counter == 0 )
285+
vx = vz = 0.0;
206286

207287
if (mode == 's') {
208-
cout << "receive: ";
209-
if ((nReadBytes = recv(server, buffer, bufsize, 0)) == SOCKET_ERROR)
210-
break;
211-
212-
vx = ((buffer[1] - '0')) / 10.0;
213-
if (buffer[0] == '-')
214-
vx *= -1;
288+
if ((nReadBytes = recv(server, buffer, bufsize, 0)) == SOCKET_ERROR) {
289+
if (WSAGetLastError() != WSAETIMEDOUT)
290+
break;
291+
else if(counter > 0)
292+
counter--;
293+
}
294+
else {
295+
counter = 3;
215296

216-
vz = ((buffer[4] - '0')) / 10.0;
217-
if (buffer[3] == '-')
218-
vz *= -1;
297+
vx = ((buffer[1] - '0')) / 10.0;
298+
if (buffer[0] == '-')
299+
vx *= -1;
219300

220-
cout << "vx = " << vx << ", vz = " << vz << endl;
301+
vz = ((buffer[4] - '0')) / 10.0;
302+
if (buffer[3] == '-')
303+
vz *= -1;
304+
}
221305
}
222306
else {
223307
char c = 0;
224308
c = cvWaitKey(30);
225309
switch (c) {
226-
case 'w': vx = +1; break;
227-
case 's': vx = -1; break;
228-
case 'a': vz = +1; break;
229-
case 'd': vz = -1; break;
230-
default: vx = 0; break;
310+
case 'w': vx = +1; break;
311+
case 's': vx = -1; break;
312+
case 'a': vz = +1; break;
313+
case 'd': vz = -1; break;
314+
default: vx = 0; break;
231315
}
232316
}
233317

@@ -253,6 +337,8 @@ int main()
253337
int velR = (int)(vr*Create_MaxVel);
254338

255339
robot.DriveDirect(velL, velR);
340+
341+
cvWaitKey(40);
256342
}
257343

258344
if (mode == 's' || mode == 'c') {

0 commit comments

Comments
 (0)