Robocup-2021-vm/main.asv at master · Ceimtun-RAS/Robocup-2021-vm · GitHub

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
%% Configure Robot
configureRobot;
%% Move to Section 1
% Section 1 pos = [0.38 0.05 0.68]
% rot = [0 pi pi/2]
sec1_pos = [0.38 0.05 0.68]
sec1_rot = [0 pi pi/2]
moveRobot(sec1_pos, sec1_rot, robot, jointSub, trajAct, trajGoal);
%% Identify, Classify and Move Objects in Section 1
close all;
% Get RGB Image
rgbImgSub = rossubscriber('/camera/color/image_raw');
curImage = receive(rgbImgSub);
rgbImg = readImage(rgbImgSub.LatestMessage);
% Get Depth Image
rgbDptSub = rossubscriber('/camera/depth/image_raw');
curDepth = receive(rgbDptSub);
depthImg = readImage(rgbDptSub.LatestMessage);
% Load YOLO Net
load('trained_final.mat', 'net');
% get prediction image
[prediction, bboxes, scores, labels] = net.predict(rgbImg);
i = 1;
box = bboxes(i, :);
% get object position
figure
imshow(prediction)

figure
imshow(depthImg)
hold on;
centroid = @(bbox) [bbox(1) + bbox(3) * 0.5, bbox(2) + bbox(4) * 0.5];
box = centroid(box);
plot(box(1)*1.78,box(2), '*');
box_pos = [box(1)*1.78 box(2)];
robot_pos = sec1_pos;
corrected_pos = correctPosition(box_pos, );
robot_pos = [sec1_pos(1)-(-0.034) sec1_pos(2)+(0.15) 0.15]
moveRobot(robot_pos, sec1_rot, robot, jointSub, trajAct, trajGoal);
%%

rgbDptSub = rossubscriber('/camera/depth/image_raw');
curDepth = receive(rgbDptSub);
depthImg = readImage(rgbDptSub.LatestMessage);
figure
imshow(depthImg)


% th = 5;
% pos_correction = sec1_pos;
%
% % correction
% u = correctPosition(box_pos,dimension)/3;
% % move
% pos_correction = pos_correction+[u 0];
% moveRobot(pos_correction, sec1_rot, robot, jointSub, trajAct, trajGoal);
% rgbDptSub = rossubscriber('/camera/depth/image_raw');
% curDepth = receive(rgbDptSub);
% depthImg = readImage(rgbDptSub.LatestMessage);
% imshow(depthImg)
% hold on;
% hold on;


% Get Depth Image


%% Move to Section 2
%% Identify, Classify and Move Objects in Section 2
%% Move to Section 3
%% Identify, Classify and Move Objects in Section 3
%% Move to Section 4
%% Identify, Classify and Move Objects in Section 4
%% Move to Section 5
%% Identify, Classify and Move Objects in Section 5
%% Look for remaining objects
%% Identify, Classify and Move Remaining Objects
%% Stop Movement and Finish Program