-
Notifications
You must be signed in to change notification settings - Fork 1
/
NavigationOutOfRangeTest.cxx
101 lines (78 loc) · 4.11 KB
/
NavigationOutOfRangeTest.cxx
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
/*=========================================================================
Program: BRP Prostate Robot: Testing Simulator (Client)
Language: C++
Copyright (c) Brigham and Women's Hospital. All rights reserved.
This software is distributed WITHOUT ANY WARRANTY; without even
the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
PURPOSE. See the above copyright notices for more information.
Please see
http://wiki.na-mic.org/Wiki/index.php/ProstateBRP_OpenIGTLink_Communication_June_2013
for the detail of the testing protocol.
=========================================================================*/
#include <string.h>
#include "igtlOSUtil.h"
#include "igtlStringMessage.h"
#include "igtlClientSocket.h"
#include "igtlSocket.h"
#include "igtlStatusMessage.h"
#include "igtlTransformMessage.h"
#include "NavigationOutOfRangeTest.h"
NavigationOutOfRangeTest::NavigationOutOfRangeTest()
{
}
NavigationOutOfRangeTest::~NavigationOutOfRangeTest()
{
}
NavigationOutOfRangeTest::ErrorPointType NavigationOutOfRangeTest::Test()
{
int queryCounter = 0;
igtl::MessageHeader::Pointer headerMsg;
headerMsg = igtl::MessageHeader::New();
std::cerr << "MESSAGE: ===== Step 1: START_UP =====" << std::endl;
SendStringMessage("CMD_0001", "START_UP");
ReceiveMessageHeader(headerMsg, this->TimeoutShort);
if (!CheckAndReceiveStringMessage(headerMsg, "ACK_0001", "START_UP")) return Error(1,1);
ReceiveMessageHeader(headerMsg, this->TimeoutLong);
if (!CheckAndReceiveStatusMessage(headerMsg, "CURRENT_STATUS", 1, 0, "START_UP")) return Error(1,2);
ReceiveMessageHeader(headerMsg, this->TimeoutLong);
if (!CheckAndReceiveStatusMessage(headerMsg, "START_UP", 1)) return Error(1,3);
std::cerr << "MESSAGE: ===== Step 2: PLANNING =====" << std::endl;
SendStringMessage("CMD_0002", "PLANNING");
ReceiveMessageHeader(headerMsg, this->TimeoutMedium);
if (!CheckAndReceiveStringMessage(headerMsg, "ACK_0002", "PLANNING")) return Error(2,1);
ReceiveMessageHeader(headerMsg, this->TimeoutLong);
if (!CheckAndReceiveStatusMessage(headerMsg, "CURRENT_STATUS", 1, 0, "PLANNING")) return Error(2,2);
std::cerr << "MESSAGE: ===== Step 3: CALIBRATION =====" << std::endl;
SendStringMessage("CMD_0003", "CALIBRATION");
ReceiveMessageHeader(headerMsg, this->TimeoutMedium);
if (!CheckAndReceiveStringMessage(headerMsg, "ACK_0003", "CALIBRATION")) return Error(3,1);
ReceiveMessageHeader(headerMsg, this->TimeoutLong);
if (!CheckAndReceiveStatusMessage(headerMsg, "CURRENT_STATUS", 1, 0, "CALIBRATION")) return Error(3,2);
igtl::Matrix4x4 matrix1;
igtl::IdentityMatrix(matrix1);
SendTransformMessage("CLB_0004", matrix1);
ReceiveMessageHeader(headerMsg, this->TimeoutMedium);
if (!CheckAndReceiveTransformMessage(headerMsg, "ACK_0004", matrix1)) return Error(3,3);
// TODO: How can we differenciate Error(3,2) and Error(3,3)?
ReceiveMessageHeader(headerMsg, this->TimeoutLong);
if (!CheckAndReceiveStatusMessage(headerMsg, "CALIBRATION", 1)) return Error(3,5);
std::cerr << "MESSAGE: ===== Step 4: TARGETING =====" << std::endl;
SendStringMessage("CMD_0005", "TARGETING");
ReceiveMessageHeader(headerMsg, this->TimeoutMedium);
if (!CheckAndReceiveStringMessage(headerMsg, "ACK_0005", "TARGETING")) return Error(4,1);
ReceiveMessageHeader(headerMsg, this->TimeoutLong);
if (!CheckAndReceiveStatusMessage(headerMsg, "CURRENT_STATUS", 1, 0, "TARGETING")) return Error(4,2);
ReceiveMessageHeader(headerMsg, this->TimeoutLong);
if (!CheckAndReceiveStatusMessage(headerMsg, "TARGETING", 1)) return Error(4,3);
igtl::Matrix4x4 matrix3;
igtl::IdentityMatrix(matrix3);
matrix3[2][3] = 5000; // +5000 in S-I direction. Should be out of range.
SendTransformMessage("TGT_0006", matrix3);
ReceiveMessageHeader(headerMsg, this->TimeoutMedium);
if (!CheckAndReceiveTransformMessage(headerMsg, "ACK_0006", matrix3)) return Error(4,4);
ReceiveMessageHeader(headerMsg, this->TimeoutLong);
// Should receive configuration error
if (!CheckAndReceiveStatusMessage(headerMsg, "TARGET", 10)) return Error(4,6);
//ReceiveMessageHeader(headerMsg, this->TimeoutMedium);
return SUCCESS;
}